相关性扫描匹配CSM与分支限界

一家之言，仅作分享，如有不合理或需要改进的地方，欢迎各位讨论。

前言

作者在先前激光SLAM项目中借鉴的是karto中的匹配方法相关性扫描匹配，在google的cartographer中的前端匹配方法也是相关性扫描匹配，但是利用了分支界定进行了加速。这篇博客将对CSM与分支界定进行介绍分享。

一、相关性扫描匹配

1、ScanMatch流程图

2、匹配过程

匹配思路其实十分简单，就是将历史点云数据构建为一个参考栅格地图，再将当前激光雷达摆放在栅格地图上（遍历位置与角度），哪一个摆放位姿下的当前点云与参考栅格地图匹配得分最高，那个位姿就是估计的位姿。

2.1、构建局部地图

扫描匹配的方式是$ScanToMap$，其中$submap$是一个grid坐标系中的矩形区域。

1. 取当前帧激光传感器的位置信息，即里程计估计的值。
2. 以当前帧激光传感器的位置为中心，构建一个矩形栅格地图。
3. 将历史帧中的激光点云坐标转换后投影在该栅格地图中。

2.2、计算匹配得分response

1. 以一定的角度分辨率和角度偏移值，建立一张角度偏离量查找表格, 获取不同角度偏移量下，每帧激光的每束激光相对于中心点位置的坐标。这样做是避免三角函数重复计算，减少计算量。
2. 对角度和二维的平移值，设置搜索范围与搜索分辨率，粗匹配时计算位置协方差；使用粗匹配输出的坐标均值再次进行精匹配，设置搜索范围与分辨率，精匹配时计算角度协方差。
3. 相应值$response$的计算：即当前scan在参考栅格地图的占用值的平均值。
4. 如果多个位姿的响应值达到阈值要求，求其平均值。

2.3、计算匹配协方差

计算角度协方差

计算位置协方差

协方差矩阵

二、分支限界

（最近比较忙，后面更新补充进去）