Cartographer 代码逻辑

根据cartographer资料自己整体

整体理论概述 

    里面有scan与submap的匹配，这部分的匹配方法是有scan_matching 中的RealTimeCorrelationScanMatcher实现的。还有一个匹配是在完成闭环检测的全局位姿优化的时候会涉及到点集的匹配（后台程序），这个部分的点集匹配算法使用的是FastCorrelationScanMatcher，两个部分都会涉及到优化算法，优化算法都是使用CeresScanMatcher来实现的。 

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的位姿。(需要注意一下，虽然论文中说了没用particle filter，但是用了kalman filter，这两者是有区别的，后者的kalman filter是采用加权求平均的方法估计位姿)

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

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

groundtruth: 这部分是系统入口(???似乎是这样的，反正包含了main函数，但是其实cartographer只是为了cartographer_ros提供接口而已，所以那只是一个测试的吧。)

以上部分很多在别人看来都是多余的（比如io，groundtruth，），所以有人做了简化版的cartographer，见hitcm大神的blog .

 

4.     mapping_2d代码逻辑

关于里面有很多的proto文件，这儿进行一些说明。这里的这些proto文件呢是google的Protocol Buffers，它是Google出品并开源的语言和平台均中立的数据序列化和反序列化工具。详情参见这个blog 其实就是一些序列号，反序列化的通信文件。其实也是一些接口文件。

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的原理及实现详细地讲解清楚并不是短短两千字能完成的。为此，本文概述了Cartographer的理论及简要的梳理了cartographer源码的逻辑，目的在于起到抛砖引玉的作用，进而有利于选择性的研究相关理论及实现。Cartographer的重点内容是融合多传感器数据的局部submap创建以及用于闭环检测的scan match策略。重点内容对应的实现是：1）基于UKF的多传感器数据融合对应cartographer/kalman_filter目录下的文件；2）scan match策略对应cartographer/mapping_2d/scan_matching目录下的文件。

 

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

 

【作者博客】http://remyspot.blog.51cto.com/

 

【版权声明】泡泡机器人SLAM的所有文章全部由泡泡机器人的成员花费大量心血制作而成的原创内容，希望大家珍惜我们的劳动成果，转载请务必注明出自【泡泡机器人SLAM】微信公众号，否则侵权必究！同时，我们也欢迎各位转载到自己的朋友圈，让更多的人能进入到SLAM这个领域中，让我们共同为推进中国的SLAM事业而努力！

【注】商业转载请联系刘富强（liufuqiang_robot@hotmail.com）进行授权。普通个人转载，请保留版权声明，并且在文章下方放上“泡泡机器人SLAM”微信公众账号的二维码即可。

 

cartographer笔记

cartographer采取的是图优化框架，基于google的Ceres构建problem优化，4线程后端优化。

1.运动预测部分
catographer利用IMU构建预测模型，ScanMatcher与Odom（可选）构建观测模型，采取UKF进行运动预测。
      2.ScanMatcher部分

cartographer采取的双搜索方式进行，先用一次real-time correlative scan matcher（三维窗口遍历寻优），再构建优化等式，利用Ceres优化求解。（栅格概率occupied_space_cost_functor_weight，T的偏差TranslationDeltaCostFunctor，R的偏差RotationDeltaCostFunctor）

3.submap说明
cartographer采用submap概念，依据一定数量的scan初始化一个submap，依据窗口大小，插入newScan，更新submap，有子图缓存，会占用内存。

4.LoopClosureCheck
cartographer依据pose和distance信息创建localMap，scanMatcher（real-time correlative scan matcher）确定。
1）依据当前的Vertex，从Graph中找到与之相邻的所有Vertex（一定距离范围内）。
2）采取广度优先搜索的方式，将相邻（next）与相连（adjacentVertices）添加进nearLinkedScans。
3）从sensorManager中取从前到后，依据ID序号挑选与当前在一定距离范围内，且不再nearLinkedScans中的candidateScans，当数量达到一定size，返回。
4）LoopScanMatcher进行scanToMap的匹配，当匹配response和covariance达到一定要求认为闭环检测到，得到调整的correctPose。
5）Add link to loop：调整边（全局闭环）
6）触发correctPose：spa优化
7）cartographer类似Real-time correlative scan matcher，引入了branch and bound的方式，加入了闭环的查找。依据多分辨率多层的树形结构，单枝生长的方式（branch），及时剪枝操作（bound），深度优先搜索确定闭环。添加相应闭环约束，构建优化问题，利用Ceres优化。

5.Huber robust error function
在统计学角度，Huber损失函数是一种使用鲁棒性回归的损失函数，它相比均方误差来说，对异常值不敏感，常常被用于分类问题上。

下面给出Huber函数的定义：

这个函数对于小的a值误差是二次的，而对大的值误差函数是线性的。变量a表述residuals，用以描述观察值与预测值之差：a = y - f(x)，因此我们可以将上面的表达式写成下面的形式：

 

Huber loss (green, delta=1) and squared error loss (blue) as a function of y - f(x)

像上面定义的那样，在Huber损失函数的最小值在a=0周边邻域上是凸的，huber损失函数将拓展了 a=-delta 和 a = delta 上的微分到仿射函数上。这些特性允许结合均值无偏的敏感性、均值的最小变化估计器（二次损失函数）和无偏中值估计器的鲁棒性（绝对值损失函数）。

Huber损失函数常常用于鲁棒性系统分析，M元估计和适应性建模。

6.Branch-and-bound （分支限界算法） 

http://blog.csdn.net/u013007900/article/details/45915067 
       http://blog.sina.com.cn/s/blog_5caa94a00100el1o.html

参考链接： http://blog.csdn.net/zyh821351004/article/details/52421005?locationNum=1

代码逻辑

参考链接：点击打开链接  http://note.youdao.com/noteshare?id=bf5c2a00496f66e88ecc200f8af8dde5

需要弄清楚到底在这个优化过程中需要的constraints来自于哪些东西？什么submaps，submap，matching_submap，insert_submap这些都是用来干什么的？
KartoSLAM的解析如下：http://blog.csdn.net/zrjust1043/article/details/66970557
KartoSLAM和Carto的比较如下：http://blog.csdn.net/zrjust1043/article/details/66970452
1)整体调用框架
OK，let's begin this function： this is pseudo main function。so it is easy for this function. for four times(just four poses as a submap).
as said in system overview of the orginal paper: when a submap is Finnished，it takes part in scan matching for loop closure。

the goal is to find the best matching for loop 。最终会有一个RunFinalOptimization。

好的，让我们开始这个功能：这是伪主要功能，所以这个功能很简单。 四次（只有四个姿势作为子图）。如原始论文的系统概述中所述：当子图是Finnished时，它将参与循环闭包的扫描匹配。目标是找到循环的最佳匹配

注意：
以上截图来自于：sparse_pose_graph_test.cc中的一种情况TEST_F(SparsePoseGraphTest, NoOverlappingScans)
for循环四次之后调用优化函数，sparse_pose_graph_->RunFinalOptimization()函数，调用这个函数之后会再次调用sparse_pose_graph_->RunOptimization(),这个优化函数会调用
optimization_problem_.Solve(constraints_, &submap_transforms_);
//vector<Rigid2d> submap_transforms_需要的两个参数是constraint，和submap_transforms_
//vector<Constraint> constraints_
需要的两个参数属于sparse_pose_graph_,这两个参数会在AddScan中被修改。
MoveRelative中的参数movement（包括vector（平移），rotation（旋转）刚开始为0）是通过随机数生成的（distribution（rng））。也就是进行数据的模拟，生成数据。为了模拟真实的数据，加入了噪声

 

2)加入噪声的模拟运动，生成的数据如下：

 

MoveRelativeWithNoise（）中的movement是随机数生成的，noise是通过Identity()这个函数生成的，涉及到调用Eign::Rotation2D<FloatType>::Identity（这个函数不是很清楚操作结果是什么）

模拟生成一个scan laser 数据之后，接下来的工作就是将其插入到子图中去。所以调用了submaps_->InsertLaserFan（将scan转换到submap下的坐标系）。之后调用sparse_pose_graph->AddScan(),按照泡泡机器人中的说法，这个sparse_pose_graph->AddScan()是对新进的laser
 fan进行闭环检测及在适当的时候进行全局优化。但是在论文中的IV. Local 2D SLAM的C. Ceres scan mathing 部分有说到：Prior to insert a scan into a submap，the scan pose is optimized relative to the current local submap using a Ceres-based scan matcher。所以在submaps_->InsertLasrFan()这部分已经完成了局部的匹配。

3)只是简单的将scan插入到submap中，局部的匹配
submaps_submaps_->InsertLasrFan()这个函数的调用过程如下：

在submaps_submaps_->InsertLasrFan()这个函数内部又调用了laser_fan_inserter_.Insert()这个函数。laserfan和scan是一个概念吗？似乎不是的，laserfan是纯粹的将激光点中hit点和miss点分开吗？？如果不同，那么局部的匹配并不是在InsertLaserFan中实现的！！！（似乎就是一个概念，因为在submap中的关于一个laser_fan索引，所以也应该是scan）
laser_fan_inserter_.Insert()这个函数实现如下：个人理解里面的CHECK_NOTNULL等等的这些check操作里面会涉及到局部的匹配。

在insert这个函数内部又调用了ApplyLookupTable（）这个是在real-time-correlative-scan-matching那篇论文里面讲到的。
在局部的匹配完成之后（？？？），会调用这个sparse_pose_graph->AddScan()是对新进的laser fan进行闭环检测及在适当的时候进行全局优化。

其中注意看关于参数的注释，matching_submap和insertion_submaps是在进行InsertLaserFan之前取出来的，
那么具体的AddScan的流程情况见下一节。
4)闭环检测以及constraint的计算（AddScan函数的操作）

这里的laser_scan就是当前pose_estimate对应的激光点集（这个pose_estimate是有噪声的测量数据，运动方程得到）。

AddScan中首先进行的是这么一句操作，在我的理解看来就是将局部的位姿转为全局的位姿。转为全局的之后才能进行闭环检测？？？
这里的GetLocalToGlobalTransform下面的注释里面解释是将局部的连续的没有闭环检测的地图转为 不连续的，具有回环检测的全局的图的转换。也就是求出这个转换关系。
AddScan中有Submaps* submaps ，Submp* matching_submap, vector<Submap*>& insertion_submaps。这三个关于子图的变量还不是很清楚含义。查看他们的数据流方向。

AddScan中调用ComputeConstraintsForScan来计算constraints。（泡泡机器人说：ComputeConstraintsForScan对新近laser fan信息进行处理并启动闭环检测scan match以及计算其约束，进而将约束添加到位姿优化目标中）其中的启动闭环检测是在什么地方呢？
在ComputeConstrainsForScan中，通过constraint_builder_对象完成闭环检测的scan match以及约束计算。

在ComputeConstraintsForScan（）中处理了一些信息之后，调用了SparsePoseGraph::ComputeConstraints()

调用ComputeConstraints计算一个scan和一个submap的constraint

在SparsePoseGraph::ComputeConstraint()中的操作如下：调用了包sparse_pose_graph中的constraint_builder_MaybeAddGlobalConstraint。

泡泡机器人中说constraint_builder_.MaybeAddGlobalConstraint以及MaybeAddConstraint会是回环检测的部分吗？是的，深入进去之后调用了fastcorrelativescanmatcher这个匹配函数。这个函数进行了回环检测（scan-match）
这个函数的实现如下：在这个实现下调用了另一个红圈画着的函数：

红圈画着的函数的实现如下：它将constraint_builder的ComputeConstraint（）作为参数传入这个函数中。其中ComputeConstraint就是里面说的work_item.

上面的ScheduleSubmapScanMatcherConstructionAndQueueWorkItem只是一个类似于调度函数的函数，具体的操作还是在constraint_builder的ComputeConstraint（）中。关于constraint_builder的ComputeConstraint（）的具体实现如下：

 

 

在constraint_builder的ComputeConstraint（）中，调用了fast_correlative_scan_matcher进行了匹配，也就是进行了回环检测。返回进行回环检测之后的分数。便于之后判断是否达到了loop-closure。
以上就是SparsePoseGraph中的ComputeConstraint计算完成之后返回到ComputeConstraintForScan这个函数，就会调用NotifyEndOfScan来通知所有的准备工作已经完成，可以进行队列的处理了，HandleScanQueue()就是进行调用工作的。HandleScanQueue()的调用如下：

里面涉及到将constraint_builder_.WhenDone函数作为参数传入。同时也涉及到SparsePoseGraph的RunOptimization，WhenDone的实现如下：

RunOptimization函数在SparsePoseGraph中的实现如下：

以上就是整个图的建立过程，但是还不是很清楚，所以再看看论文：A tutorial on graph-based SLAM
还有就是关于scan-match的文章是：real time correlative scan matching
5）优化函数的实现（optimization_problem_.Solve）
对于论文中在closing loop中需要计算的optimization problem，它需要的参数是submap poses，scan poses，the relative pose where in the submap coordinate frame the scan was matched。还有一个就是方差矩阵（the covariance matrices），所以这些具体是如何计算的呢？
BA和pose graph类似，但是pose graph是BA的改进版，在BA中会有landmark顶点（带有相机位姿和空间点的图优化），但是在pose graph里面把landmark当做对位姿节点的约束，构造只优化相机位姿的pose graph。
对于优化，首先是先构造目标函数，目标函数构造之后再调用不同的算法（g2o,ceres）等算法来求解。
重点还是没理解具体怎么构造约束以及目标函数！！