讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言(Scan match原理)

通过前面你的一系列博客，已经知道 Cartographer 中的概率栅格图是如何建立的了。不过需要注意的一点是该地图并不仅仅是保存下来给来看的，其还会被点云扫描匹配使用到，点云扫描匹配目的是估算位姿(该部分内容后面会详细讲解)。在 slam 中分为 Scan match 与 Point cloud match，通常情况下前者指的就是2D扫描匹配，后者指的是3D点云匹配。为了方便后续代码的理解，这里简单介绍一下扫描匹配的原理，先来看下图(理想位姿)：

图一：理想位姿

1、方格 → 所有的栅格组成。
2、黑色方格 → 障碍物，光子会被阻碍，形成点云数据。
3、白色方格 → 空闲区域，表示没有障碍物，光子可以直接穿过。
4、紫色多边形 → 代表机器人，当然也可以理解为雷达传感器原点，或者点云数据的原点。
5、黄色圆形 → 点云数据。

从上图来看，可见所有的点云数据都与障碍物重叠了，说明此时估算出来的位姿十分正确，这里称为理想位置。但是系统在运行的过程中并非是这样的。这里假设我们由传感器数据(Imu、GPS、里程计) 等估算出来的位姿如下所示：

图二：初始位姿 容易看出上图估算出来的位姿是存在偏差的，假设传感器估算出来的初始位姿，是不太准确的，那么问题就来了，如何根据初始位姿求解出理想位姿呢？首先，通常来说理想位姿需要在初始位姿的一定范围之内(也就是说，传感器的误差不能太大了)，基于这个前提，再来分析上图。

根据图示可以看出初始位姿与理想位姿在位置与姿态上都存在差异，也就是说由初始位姿变换到理想位姿，其平移与旋转可能都需要发生改变。基于这个原理，最简单的一种扫描方式就是暴力匹配，也就是在位移与角度上维度上进行遍历，流程如下：

1、设定初始位姿
2、位移扫描匹配遍历
3、角度扫描匹配遍历。
4、评分规则

初始位姿可以直接由传感器估算出来，先来看下图，

图三：位姿遍历

绿色区域 1-6 表示需要遍历的区域，也就是理想位姿的平移处于该区域内，也就是位移扫描匹配需要遍历绿色区域1-6。蓝色的箭头表示角度遍历的方向。该箭头的起点就是角度开始遍历的起点，终点就是角度遍历的终点。

如上图所示，从1号区域