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