Cartographer与Cartographer_ros学习总结

首先学习的版本是以 0.2.0为准的
cartographer：https://github.com/cartographer-project/cartographer/tree/0.2.0
DOC文档：https://google-cartographer.readthedocs.io/en/latest/
cartographer_ros:https://github.com/cartographer-project/cartographer_ros/tree/0.2.0
DOC文档：https://google-cartographer-ros.readthedocs.io/en/latest/

2017年版的中文注释代码：https://github.com/slam-code/cartographer

1.Cartographer理论概述

Cartographer主要理论是通过闭环检测来消除构图过程中产生的累积误差[1]。用于闭环检测的基本单元是submap。一个submap是由一定数量的laser scan构成。将一个laser scan插入其对应的submap时，会基于submap已有的laser scan及其它传感器数据估计其在该submap中的最佳位置。submap的创建在短时间内的误差累积被认为是足够小的。然而随着时间推移，越来越多的submap被创建后，submap间的误差累积则会越来越大。因此需要通过闭环检测适当的优化这些submap的位姿进而消除这些累积误差，这就将问题转化成一个位姿优化问题。当一个submap的构建完成时，也就是不会再有新的laser scan插入到该submap时，该submap就会加入到闭环检测中。闭环检测会考虑所有的已完成创建的submap。当一个新的laser scan加入到地图中时，如果该laser scan的估计位姿与地图中某个submap的某个laser scan的位姿比较接近的话，那么通过某种 scan match策略就会找到该闭环。Cartographer中的scan match策略通过在新加入地图的laser scan的估计位姿附近取一个窗口，进而在该窗口内寻找该laser scan的一个可能的匹配，如果找到了一个足够好的匹配，则会将该匹配的闭环约束加入到位姿优化问题中。Cartographer的重点内容就是融合多传感器数据的局部submap创建以及用于闭环检测的scan match策略的实现。

2.开源代码逻辑

Google开源的代码包含两个部分：cartographer[2]和cartographer_ros[3]。cartographer主要负责处理来自雷达、IMU和里程计的数据并基于这些数据进行地图的构建，是cartographer理论的底层实现。cartographer_ros则基于ros的通信机制获取传感器的数据并将它们转换成cartographer中定义的格式传递给cartographer处理，与此同时也将cartographer的处理结果发布用于显示或保存，是基于cartographer的上层应用。

3. cartographer代码结构

common：定义了基本数据结构以及一些工具的使用接口。

sensor：定义了雷达数据及点云等相关的数据结构。

transform：定义了位姿的数据结构及其相关的转换。

kalman_filter: 主要通过kalman滤波器完成对IMU、里程计及基于雷达数据的估计位姿的融合，进而估计新进的laser scan的位姿。

mapping：定义了上层应用的调用接口以及局部submap构建和基于闭环检测的位姿优化等的接口。

mapping_2d和mapping_3d：对mapping接口的不同实现。

4. mapping_2d代码逻辑

4.1 cartographer::mapping_2d:: GlobalTrajectoryBuilder

cartographer::mapping_2d::GlobalTrajectoryBuilder类主要实现了接收处理上层应用传递的传感器数据的主要接口：

（1） AddImuData用于接收处理上层应用传递的IMU数据。

（2） AddOdometerPose用于接收处理上层应用传递的里程计数据。

（3） AddHorizontalLaserFan用于接收处理上层应用传递的雷达数据。

其中包含重要的对象成员：

（1） artographer::mapping_2d::LocalTrajectoryBuilder类的对象local_trajectory_builder_用于完成局部submap的构建。

（2） cartographer::mapping_2d::SparsePoseGraph类的对象sparse_pose_graph_用于完成闭环检测及全局位姿优化。

在AddImuData和AddOdometerPose函数的实现中会将接收的相应传感器数据传递给local_trajectory_builder_对象处理。在AddHorizontalLaserFan函数的实现中则将新进的laser fan传递给local_trajector_builder_对象用于局部submap构建，如果该laser fan被成功插入到某个submap，那么该laser fan被插入后的相关信息则被传递给sparse_pose_graph_对象用于基于闭环检测的全局位姿优化。

4.2 cartographer::mapping_2d::LocalTrajectoryBuilder

cartographer::mapping_2d::LocalTrajectoryBuilder类主要完成局部submap的构建。其提供了接收处理传感器数据的public函数：

（1） AddImuData用于处理IMU数据。

（2） AddOdometerPose用于处理里程计数据。

（3） AddHorizontalLaserFan用于处理雷达数据。

以及包含了一些重要的private成员：

（1） ScanMatch成员函数基于submap已有的laser fan估计当前laser fan在submap中的位置。

（2） cartographer::kalman_filter::PoseTracker类的对象
pose_tracker_用于融合基于雷达数据的laser fan的局部估计位姿、IMU数据以及里程计数据，进而估计出较优的laser fan的位姿。

在AddImuData和AddOdometerPose函数中会将IMU数据和里程计数据传递给pose_tracker_进行处理。pose_tracker通过UKF不断融合IMU和里程计数据进而更新当前位姿，因此通过pose_tracker可以获取当前laser fan的估计位姿的一个较好的初始化值。进一步的，在AddHorizontalLaserFan函数中会调用ScanMatch，ScanMatch函数中通过在submap中局部匹配得到的当前laser fan的估计位姿被pose_tracker_用来调整该laser fan的初始化值。这样pose_tracker_通过融合多传感器数据，进而能够估计出较优的laser fan的位姿。

4.3 cartographer::mapping_2d::SparsePoseGraph

cartographer::mapping_2d::SparsePoseGraph类主要完成基于闭环检测的全局位姿优化。其提供了接收处理新进被插入到submap的laser fan相关信息的public函数：

（1） AddScan 对新进的laser fan进行闭环检测及在适当的时候进行全局优化。

以及一些重要的私有成员：

（1） ComputeConstraintsForScan对新近laser fan信息进行处理并启动闭环检测scan match以及计算其约束，进而将约束添加到位姿优化目标中。

（2） AddWorkItem将laser fan与ComputeConstraintsForScan绑定，并将任务加入到队列中。

（3） HandleScanQueue依此调度队列中的任务。

（4） sparse_pose_graph::ConstraintBuilder constraint_builder_ 用于完成laser fan的scan match以及约束计算。

（5） RunOptimization优化目标。

在AddScan函数中会将laser fan相关信息与ComputeConstraintsForScan函数绑定，并将绑定好的任务通过AddWorkItem函数加入到队列中。HandleScanQueue函数则依次调度队列中的任务。第一次调用AddWorkItem时会直接启动ComputeConstraintsForScan任务，且在第一次ComputeConstraintsForScan任务时启动HandleScanQueue调度。在ComputeConstrainsScan中，通过constraint_builder_对象完成闭环检测的scan match以及约束计算。当所有约束计算完成时，则会进行RunOptimization优化目标。

4.4 Scan
Match

LocalTrajectoryBuilder中的scan match策略与SparsePoseGraph中的scan match策略是不同的。前者使用scan_matching::RealTimeCorrelativeScanMatcher，后者则使用scan_matching::FastCorrelativeScanMatcher。二者的目标优化均是由scan_matching::CeresScanMatcher完成。

5. 总结

Cartographer的重点内容是融合多传感器数据的局部submap创建以及用于闭环检测的scan match策略。重点内容对应的实现是：1）基于UKF的多传感器数据融合对应cartographer/kalman_filter目录下的文件；2）scan match策略对应cartographer/mapping_2d/scan_matching目录下的文件。

～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～
Hector slam：

Hector slam利用高斯牛顿方法解决scan-matching问题，对传感器要求较高。

缺点：需要雷达（LRS）的更新频率较高，测量噪声小。所以在制图过程中，需要robot速度控制在比较低的情况下，建图效果才会比较理想，这也是它没有回环（loop close）的一个后遗症；且在里程计数据比较精确的时候，无法有效利用里程计信息。

优点：不需要使用里程计，所以使得空中无人机及地面小车在不平坦区域建图存在运用的可行性；利用已经获得的地图对激光束点阵进行优化, 估计激光点在地图的表示,和占据网格的概率；利用高斯牛顿方法解决scan-matching 问题，获得激光点集映射到已有地图的刚体变换（x,y,theta）；为避免局部最小而非全局最优，使用多分辨率地图；导航中的状态估计加入惯性测量系统（IMU），利用EKF滤波；

补充：11-11 hector_slam通过最小二乘法匹配扫描点，且依赖高精度的激光雷达数据，因此扫描角很小且噪声较大的Kinect是不行的，匹配时会陷入局部点，地图比较混乱。

gmapping：

scanmatch方法：链接

gmapping是目前应用最广的2D slam 方法，利用RBPF方法，故需要了解粒子滤波算法。scan-match方法在于估计机器人位置（pose），利用梯度下降的方法，在当前构建的地图，与当前的激光点，和机器人位置（pose）为初始估计值。

粒子滤波的方法一般需要大量的粒子来获取好的结果,但这必会引入计算的复杂度;粒子是一个依据过程的观测逐渐更新权重与收敛的过程,这种重采样的过程必然会代入粒子耗散问题(depletion problem), 大权重粒子显著,小权重粒子会消失(有可能正确的粒子模拟可能在中间的阶段表现权重小而消失).自适应重采样技术引入减少了粒子耗散问题 , 计算粒子分布的时候不单单仅依靠机器人的运动(里程计),同时将当前观测考虑进去, 减少了机器人位置在粒子滤波步骤中的不确定性. (FAST-SLAM 2.0 的思想，可以适当减少粒子数)

缺点：依赖里程计（odometry），无法适用无人机及地面小车不平坦区域；无回环；

优点：在长廊及低特征场景中建图效果好；

补充：（11-11）据说gmapping可以用Kinect转换出来的激光雷达数据建图，但是苦于没有里程计，所以没有尝试。（如果你有试过，请把结果告诉我，谢谢）

实验表明：在大地图，低特征（distinctive landmarks）场景中，hector的建图误差高于gmapping。这是由于hector过分依赖scan-match。特别是在长廊问题中，误差更加明显。

cartographer：

cartographer是Google的实时室内建图项目，传感器安装在背包上面，可以生成分辨率为5cm的2D格网地图。

获得的每一帧laser scan数据，利用scan match在最佳估计位置处插入子图（submap）中，且scan matching只跟当前submap有关。在生成一个submap后，会进行一次局部的回环（loop close），利用分支定位和预先计算的网格，所有submap完成后，会进行全局的回环。

local 2D slam

A.scans

submap的构造是一个重复迭代配准scan和submap的过程。利用配准估算出pose对scan进行刚体变换，插入到submap中。

B.submaps

连续的scan用来构造submap，这里submap以概率格网的形式表现。每一个scan，在插入格网（submap）时，每一个grid有hits和miss两种情况。离scan终点最近的grid为hits，在scan原点和终点之间相交的grid为miss。之前未观察的grid分配一个概率，已观察的grid进行概率更新。

C.Ceres scan matching

把求pose的问题转换为一个求解非线性最小二乘问题，利用Ceres解决这个问题。

因为最小二乘问题是一个局部最优问题，故一个好的初值（pose初值）对求解有很大影响。因此IMU能被用来提供pose初值的旋转变量。在缺乏IMU的时候，可以用提高scan match频率或匹配精度。

closing loops
利用SPA方法优化scan和submap的pose。存储插入scan位置处对应的pose用来做回环检测。此外，当submap不在变化时，对应pose的scan和submap也被用来做回环。scan match中找到的good match其对应的pose将被用来做优化问题。

A.Optimization problem

回环优化问题构造成非线性最小二乘问题，来求解。

公式中分别为submap的pose，scan的pose，对应submap和scan的pose的相关性，及相关协方差矩阵。

B.Branch-and bround scan matching

缺点：

优点：不采用粒子滤波（PF），从而低廉设备也能获得较好的性能表现；
上文链接：https://blog.csdn.net/SLAMTEC/article/details/82907418