(02)Cartographer源码无死角解析-(48) 2D点云扫描匹配→扫描匹配基本原理讲解，代码总体框架梳理

news2024/10/5 19:14:12

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885

${\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证}$

一、前言(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号方格开始第一次遍历，Robot角度为-35°(假设平行y轴为0°)，记为 1_(-35)。需要主意的是，Robot 与点云需要一直保持相对禁止，Robot位姿如何变换，点云则需做同样的变换。在 1_(-35) 的位置上，我们希望所有的点云都能够与黑色方格(障碍物)完全匹配，但是显然是不行的，所以改变一下角度，这里假设角分辨率为5°，那么下次利用 1_(-30) 的 Robot 与点云位姿与黑色方格进行匹配，依此一下，进行位姿和角度上的遍历，如下：

1_(-35)   1_(-30)   1_(-25) ......  1_(30)   1_(35)
2_(-35)   2_(-30)   2_(-25) ......  2_(30)   2_(35)
......
6_(-35)   6_(-30)   6_(-25) ......  6_(30)   2_(35)

其可以按行遍历，也可以按列遍历，每一个上述 Robot 与点云位姿与黑色方格进行匹配之后，我们都需要进行一次评分，在这个例子中会匹配6x(70/5)=6x14=84次，也就是说这里需要进行84次评分。那么问题来了，如何进行评分，才能体现出匹配结果的优劣呢？

从上图可以很直观的看出点云与障碍物重叠度越高，则匹配效果越好。结合前面的内容，这里假设存在 $n$ 个点云，其每个点云对应的方格记为 $cell_i$ ， $cell_i$ 对应被占用的概率为 $p_i$ ，那么每次匹配的得分可以记为 $s_x=(\sum _{i=1}^np_i)/n$ 。然后认为所有匹配(84次)得分中最高的 $s_{max}$ 为最优匹配。也就是图一的理想匹配。