cartographer中的SparsePoseGraph

似乎这一系列都是我直接从有道的小结里面复制过来的。图片看不见了～

原文地址：

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。

注意：

以上截图来自于：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）等算法来求解。

重点还是没理解具体怎么构造约束以及目标函数！！