Karto_SLAM算法学习

一、代码下载地址
https://github.com/ros-perception/open_karto.git
https://github.com/ros-perception/slam_karto.git
代码分为两部分 ：open_karto、slam_karto
采用通用的图 优化方法，Karto_SLAM是基于图优化的思想，用高度优化和非迭代 cholesky分解进行稀疏系统解耦作为解。图优化方法利用图的均值表示地图，每个节点表示机器人轨迹的一个位置点和传感器测量数据集，每个新节点加入，就会进行计算更新。

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

二、扫描匹配部分
兴趣区域：
匹配的方式是scanTomap，兴趣区域就是矩形形状的submap，也可以将这块区域理解为参考模型。

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

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

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

生成submap(因为匹配的方式是ScanToMap)

生成查找表：
查找表存在的意义就是相比于暴力匹配，不要每次都重新计算每个激光数据信息，相同角度不同位置的激光数据信息只需要被索引一次
由里程计预测可以得到当前预估的姿态角，真实的姿态角必定在附近；
以一定的分辨率和偏移值对原机器人坐标系下（局部坐标系）表示的激光信息进行不同角度的映射，获得一个包含n个角度的查找表。
其中：
查找表->submap（匹配）
移动机器人自身的状态包括(x,y,θ)(x,y,θ)，通过查找表的方式有效解决了角度的问题，眼下仍需解决位置的问题，采用离散化搜索区域进行定位，并利用多分辨率提高搜索速度
响应值的计算如下描述：将查找表以一定的为宜投到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优化
第一步，首先去除那些和当前节点的时间相邻的节点，针对那些在搜索范围内，且在时间点上并不相邻的节点产生一个数据链，然后进行匹配，若响应值大于阈值，添加回环并进行全局优化。

部分参考原文链接：https://blog.csdn.net/qq_24893115/article/details/52965410