Detailed interpretation cartographer source (1) - the overall framework Introduction

1. Overview of theory Cartographer Cartographer main theory is to eliminate the accumulated errors produced by the patterning process loop detection [1]. The base unit for detecting the closed submap. A submap is composed of a number of laser scan. When inserted into a corresponding laser scan submap, estimates the optimal position in the submap based submap conventional laser scan and other sensor data. submap created in a short period of cumulative error is considered to be sufficiently small. Over time, however, after more and more submap is created, the cumulative error between the submap will be growing. It is necessary to optimize the appropriate posture detecting these submap through a closed loop thereby eliminating these errors accumulate, this problem will be transformed into a pose optimization problem. When constructing a complete submap, that is there will be no new submap inserted into the laser scan, the submap will be added to the loop detection. Closed-loop detection will consider all completed created submap. When a new laser scan is added to the map, if a laser is estimated pose with a map of the laser scan of the scan of a submap pose relatively close, then it will find the loop through some scan match strategy . Cartographer scan match strategy by taking a window of the newly added map in the vicinity of the laser scan of the estimated pose, and then look for a possible match of the laser scan in the window, if you find a good enough match, it will be the match closed constraint is added in place pose optimization problems. Cartographer's main focus is the integration of local submap create a multi-sensor data as well as strategies for implementing scan match closed loop detection. 2. Google open open code logic code consists of two parts: cartographer [2] and the cartographer_ros [3]. cartographer responsible for processing data from the radar, IMU and odometer and build maps based on these data, the underlying theory cartographer implementation. cartographer_ros sensor data is acquired based on the communication mechanism ros and converts them into the format defined cartographer cartographer passed to the processing, the processing result cartographer at the same time for display or storage of release is based on the upper-layer application cartographer. 3. cartographer code structure common: using an interface defines data structures as well as some basic tools. sensor: defines the point cloud radar data and other related data structures. transform: defines the data structure and associated posture conversion. kalman_filter: Main complete fusion of an IMU, odometer, and pose estimation based radar data by kalman filter, New further estimated pose of the laser scan. mapping: call interface defines the interface construction applications, and the upper local submap and Pose Optimization loop detection and the like. mapping_2d and mapping_3d: different implementations of the mapping interface. 4. mapping_2d code logic 4.1 cartographer :: mapping_2d :: GlobalTrajectoryBuilder cartographer :: mapping_2d :: GlobalTrajectoryBuilder principal realize the main processing interface for receiving the sensor data transmitted to an upper layer application: (1) AddImuData IMU data reception processing for transmitting the upper layer application . (2) AddOdometerPose odometry data processing means for receiving an upper layer application of the transfer. (3) AddHorizontalLaserFan radar data processing means for receiving an upper layer application of the transfer. Which contains important object members: (1) artographer :: mapping_2d :: Object class local_trajectory_builder_ LocalTrajectoryBuilder constructed for performing the local submap. (2) cartographer :: Object class sparse_pose_graph_ mapping_2d :: SparsePoseGraph for performing closed-loop detection and pose global optimization. Respective sensor data and AddOdometerPose functions implemented in AddImuData received will be passed to local_trajectory_builder_ object processing. In the implementation of the function will AddHorizontalLaserFan New local_trajector_builder_ the laser fan is passed to build objects for topical submap, if the laser fan is successfully inserted into a submap, then the related information is inserted into the laser fan were transferred to sparse_pose_graph_ objects based on global pose optimization loop detection. 4.2 cartographer :: mapping_2d :: LocalTrajectoryBuilder cartographer :: mapping_2d :: LocalTrajectoryBuilder principal completed Construction of local submap. Providing a sensor data reception processing public functions: (1) AddImuData IMU data for processing. (2) AddOdometerPose odometry data for processing. (3) AddHorizontalLaserFan for processing radar data. And it contains a number of important private member: (1) ScanMatch member function of the estimated position of the current laser fan in the submap based submap existing laser fan. (2) cartographer :: kalman_filter :: Object class pose_tracker_ PoseTracker laser based radar data for fusion of local estimators pose fan, an IMU data and odometry data, and further superior laser fan estimated pose. AddImuData and transfer functions will AddOdometerPose IMU and odometer data to the data processing pose_tracker_. pose_tracker by UKF continued integration of IMU and odometer data and then updates the current position and orientation, so you can get a better estimate of the current position and orientation of the laser fan of initialization values ​​pose_tracker. Further, the function calls in AddHorizontalLaserFan ScanMatch, ScanMatch function by matching the current partial laser submap obtained pose estimation fan pose_tracker_ be used to adjust the initial value of the Laser fan. Such pose_tracker_ by multi-sensor data fusion, and thus can be estimated superior laser fan pose. 4.3 cartographer :: mapping_2d :: SparsePoseGraph cartographer :: mapping_2d :: SparsePoseGraph principal global pose optimization is done based on the detection loop. Which provides a public function information reception processing laser fan is inserted into the New submap of: (1) AddScan of the laser fan New closed-loop detection and global optimization at the appropriate time. As well as a number of important private member: (1) ComputeConstraintsForScan newly laser fan of information to be processed and closed-loop detection scan match and start calculating its constraints, and then add the place constraints pose optimization objectives. (2) AddWorkItem ComputeConstraintsForScan binding with the laser fan, and the task added to the queue. (3) HandleScanQueue so schedule tasks in the queue. (4) sparse_pose_graph :: ConstraintBuilder constraint_builder_ laser fan for completing the scan match calculated and constraints. (5) RunOptimization optimization objectives. fan function related information ComputeConstraintsForScan AddScan function will bind laser, and bind well to the queue by task AddWorkItem function. HandleScanQueue function of the scheduled tasks in the queue sequentially. ComputeConstraintsForScan task will directly start the first call AddWorkItem, and is scheduled to start in the first HandleScanQueue ComputeConstraintsForScan task. In ComputeConstrainsScan, the detection loop is completed by a scan match constraint_builder_ computing objects and constraints. When all the constraints calculations are complete, it will be RunOptimization optimization goals. 4.4 Scan Match scan match strategy and SparsePoseGraph the scan match strategy LocalTrajectoryBuilder is different. The former uses scan_matching :: RealTimeCorrelativeScanMatcher, the latter using scan_matching :: FastCorrelativeScanMatcher. Both are objective optimization is done by scan_matching :: CeresScanMatcher. 5. To summarize the principles and implementation Cartographer clearly explain in detail just a few thousand words can not complete. Therefore, this article provides an overview of the theory and briefly reviews the Cartographer logic cartographer source, aimed, serve, thereby facilitating research and theories to achieve selective. Cartographer main focus is to create a local submap multi-sensor data fusion and for scan match strategy closed-loop detection. Key corresponding to the contents is achieved: 1) based on multi-sensor data fusion UKF file corresponding cartographer / kalman_filter directory; 2) under the document scan match policy corresponds cartographer / mapping_2d / scan_matching directory. Follow-up will also have the opportunity to focus on the content and implementation of these sort out in detail. Reference: [1] Wolfgang Hess, Damon Kohler, Holger Rapp, Daniel Andor Real-time loop closure in 2D lidar slam ICRA, 2016. [2] https://github.com/googlecartographer/cartographer [...... 3] https://github.com/googlecartographer/cartographer_ros