MIT：Karto_slam框架与代码解析

Karto_slam框架与代码解析

1、Karto_slam概况介绍

2、karto_slam具体介绍

1、scan-matcher

2、添加顶点和边的信息

3、回环检测

4、优化求解

3、关于Karto_slam简单的总结

正文如下

1、Karto_slam概况

 
 
上面两张图主要是对Karto_slam的一个整体框架，从2016年slam的大综述中，可以看到这其实就是一般slam的一个基本框架，这里指的是带优化的框架，作者2010的论文正是图优化刚开始涌入至slam的萌芽期。

关于karto_slam的一些评价 
Karto_SLAM是基于图优化的思想，用高度优化和非迭代 cholesky分解进行稀疏系统解耦作为解。图优化方法利用图的均值表示地图，每个节点表示机器人轨迹的一个位置点和传感器测量数据集，每个新节点加入，就会进行计算更新。

　　Karto_SLAM的ROS版本，其中采用的稀疏点调整（the Spare Pose Adjustment(SPA)）与扫描匹配和闭环检测相关。landmark越多,内存需求越大,然而图优化方式相比其他方法在大环境下制图优势更大，因为他仅包含点的图(robot pose)，求得位姿后再求map。

参考文献：Machado Santos J, Portugal D, Rocha R P. An evaluation of 2D SLAM techniques available in Robot Operating System[C]// IEEE Int. Symp. on Safety, Security, and Rescue Robotics. 2013:1-6.

参考资料 
Karto_SLAM整体框架： 
K. Konolige, G. Grisetti, R. K¨ummerle, B. Limketkai, R. Vincent, Efficient Sparse Pose Adjustment for 2D Mapping, In Proc. of Int. Conf. on Intelligent Robots and Systems (IROS), Oct. 2010.

scan-match 
E. B. Olson. Real-time correlative scan matching. In International Conference on Robotics and Automation, pages 4387–4393, 2009.

主函数（karto_slam）: 
ros-perception/slam_karto

包含库（open_karto）: 
ros-perception/open_karto

1、Karto_slam具体介绍

首先是整体程序框架 
 
从上图中可以看出，其实流程还是相当简单的，slam传统软实时的运行机制，每进入一帧数据，即进行处理，然后返回

1、扫描匹配

关键词： 
兴趣区域： 
匹配的方式是scanTomap，兴趣区域就是矩形形状的submap，也可以将这块区域理解为参考模型。

搜索区域： 
以里程计估计的位置为中心的一个矩形区域，用以表示最终位置的可能范围，在匹配时，遍历搜索区域，获取响应值最高的位置。

查找表： 
对于激光获得的数据信息，以一定的角分辨率和角偏移值进行投影，获取查找表，用以匹配。

Running-scans： 
实时维护的局部激光数据链，首末两帧距离在一定距离范围内，且满足一定数据规模，否则需要删除末端数据帧。维护当前局部数据链

生成submap(因为匹配的方式是ScanToMap) 
 
生成查找表： 
查找表存在的意义就是相比于暴力匹配，不要每次都重新计算每个激光数据信息，相同角度不同位置的激光数据信息只需要被索引一次 
 
由里程计预测可以得到当前预估的姿态角，真实的姿态角必定在附近； 
以一定的分辨率和偏移值对原机器人坐标系下（局部坐标系）表示的激光信息进行不同角度的映射，获得一个包含n个角度的查找表。 
其中：

查找表->submap（匹配） 
移动机器人自身的状态包括，通过查找表的方式有效解决了角度的问题，眼下仍需解决位置的问题，采用离散化搜索区域进行定位，并利用多分辨率提高搜索速度，具体理论细节可以参考09年olson的论文。 

响应值的计算如下描述：将查找表以一定的为宜投到submap上，此时的submap已有running-scans生成，并利用高斯进行模糊，假设总共有n个点被查找表击中，击中的每个点得分不同（高斯模糊的作用），累加得分并除以可以达到的最高得分
查找表->submap（粗匹配->精匹配） 
为提高搜索效率，采用多分辨率的方式，即粗匹配时采用较低的分辨率搜索得到候选区域，再对候选区域进行划分得到精确求解。 

计算节点均值 
在每次匹配过程中（粗匹配和精匹配），选取几个拥有最优响应值的位姿状态，取平均值作为匹配结果。

 

最优响应值为一个，设立一个10^-6作为容忍度，一旦有其他位姿状态的响应值与最优响应值在容忍度内，即为相等；最终的节点为它们的平均值。 
迭代： 
粗匹配（获得均值）->（作为初值）精匹配（得到最终均值）

计算节点协方差——位置协方差（粗匹配） 
 
计算节点协方差——角度协方差（粗匹配） 

上述协方差存在和理论相背的情况，主要每次计算的分量需要乘上一个响应值，具体需要再商榷。、

2、添加顶点和边

添加顶点：即关键帧的位姿 
添 加 边： 当前帧与前一帧、running-scans、near-chains建立连接。 
 
添加边主要经过三个步骤： 
1、Link to previsous scan

2、Link to Running scans 
RunningScan chain：一定数量且距当前一定距离内的激光数据链。

3、Link to other near chains 
NearChain: 以当前节点开始广度优先的方式从graph中遍历相邻的一定距离范围内所有节点，依据当前id从sensorManager中分别递增与递减寻找一定范围内的chain， 生成nearLinkScans. 
前两个步骤都好理解，主要是第三个，以当前节点为搜索中心，以一定范围（比如说4m）进行广度搜索，得到了所有关联的节点，然后利用节点生成数据链（数据链需要包含约5个ID连续的关键帧）（不包括当前结点，否则放弃该数据链），用当前节点进行匹配，若响应值达到一定阈值，产生一条边，这条边一端是当前节点，还有一端是数据链中质心距离当前节点质心最近的节点 

3、回环检测

回环检测的操作和添加邻近边类似，步骤较为繁琐： 
1、依据当前的节点, 从Graph中找到与之相邻的所有节点(一定距离范围内)

2、采取广度优先搜索的方式，将相邻（next）与相连（adjacentVertices）添加进nearLinkedScans.

3、从sensorManager中取从前到后，依据id序号挑选与当前在一定距离范围内，且不在nearLinkedScans中的candidateScans, 当数量达到一定size，返回。

4、loopScanMatcher进行scanTomap的匹配，当匹配response 和covariance达到一定要求认为闭环检测到。得到调整的correct pose。

5、Add link to loop : 调整边（全局闭环）

6、触发correctPose: spa优化 
第一步，首先去除那些和当前节点的时间相邻的节点，针对那些在搜索范围内，且在时间点上并不相邻的节点产生一个数据链，然后进行匹配，若响应值大于阈值，添加回环并进行全局优化。

4、优化求解

 
整个优化算法使用了LM优化算法，原文中，作者大篇幅的都是介绍如何得到海森矩阵，以及高效进行优化求解，LM系统如下： 
 
海森矩阵的稀疏： 

karto_slam总结

1、单从论文角度看，论文并没有关于整个框架的介绍，大部分的内容都是关于优化方法、黑森矩阵的构造等。关于扫描匹配、顶点和边、回环检测等所提甚少。

2、源代码包含主函数和库函数两部分，框架清晰，可读性强；除了eigen，不需要额外的依赖库。

3、关于karto_slam的介绍不如hector和gmapping多，可能是 没有采用主流的地图更新机制（概率地图）。只保存了pose节点和激光数据，作为pose-graph。

室内gmapping和karto比较 
gmapping 
 
karto