0. 简介

CSM在Cartographer中是比较基础且非常适合拓展的功能。他主要的步骤如下图。

主要实现的步骤为：

1）获取先验位姿，通过TF获取里程计的值，作为当前scan的预测位姿，将这个预测位姿当做扫描匹配的先验。

2）使用滑动窗口法来生成局部地图；Karto使用了一个队列保存最近24米范围内的所有雷达数据，通过将滑动窗口内的所有雷达数据写成分辨率为0.01m的栅格地图，这样就生成了局部地图。

3）通过暴力求解的方式，遍历一定范围内所有的平移与旋转的位姿，选出得分最大的一个位姿。（扫描匹配）加速方式：

4）生成角度查找表，实现暴力求解内部的两个循环(对于xˆ和yˆ)只是转换查询点。
5）多分辨率概率查询表，先粗匹配计算位置协方差，输出坐标均值，再次进行精匹配，计算角度协方差。

1. CSM原理

帧间匹配可以理解成一个：已知$x_{i-1}$ 和$m$，也测量到了$u$ 和 $z_i$ ，如何尽可能准确的求出$x_i$。即：$p(x_i|x_{i-1},u,m,z_i)$

应用高斯分布，$p(x_i|x_{i-1},u,m,z_i)=p(z|x_i,m)p(x_i|x_{i-1},u)$ 。其中$p(x_i|x_{i-1},u)$是我们熟悉的运动模型，$p(z|x_i,m)$ 是观测模型。前者容易求解；后者的计算是一个难题，容易陷入局部最优解。
  假设：第$i$次观测$Z_i$中的各个激光点（以 $z_i^k$表示）位置的概率分布是彼此独立的，则：

对上述公式两侧取对数，让乘法变加法，我们有：

公式（3）是我们的观测模型，我们通过下面的思路对其进行求解：

• 根据 前面的观测 $m$（或若干帧）建立出来的概率栅格地图！

• 按照位姿$x_i$把当前观测$z_i$投影到$m$中，把所有被击中的栅格的概率值相加，就是在栅格地图的基础上，公式(3) 的求解。 所得的即是位姿的得分，代表着在这个位姿下，当前观测 与已知环境 相一致的程度（i.e., 相关性）。得分越高，表明这个位姿越靠谱。

通过上面的介绍，我们可以总结出CSM的主要思想：
  CSM帧匹配算法基于概率栅格地图运行，每个栅格都维护一个对数形式的占据概率；对于新进来的激光scan，将scan中所有点通过一个预估位姿投影到栅格地图上，这样每个激光点都会落到一个栅格中，激光点所在栅格的对数概率值之和既为当前位姿的得分，代表着这个位姿的可信度。但是预估位姿是不准确的，我们在预估位姿附近建立一个搜索空间，通过分支定界策略加速搜索，求出得分最高的备选位姿，作为最优结果输出 。

2. CSM代码实现

这里实现了一种扫描匹配算法，用于比较两个点云并找到它们之间的最佳平移和旋转。首先，它使用给定的点云和分辨率创建一个LookupTable，并对每个点赋值，然后通过高斯模糊和归一化来处理。接着，它根据高分辨率从高分辨率LookupTable生成新的低分辨率LookupTable。然后，根据给定的旋转矩阵旋转点云，并计算一组点云在某个平移和旋转情况下与目标网格的得分。接下来，它预计算可能的平移和旋转范围，保存在一个低分辨率的概率列表中。最后，它根据高分辨率网格进行平移和旋转搜索，以获取最佳匹配的得分和转换信息。

这段代码的核心思想是利用不同分辨率的LookupTable和预计算的概率列表来寻找最佳的平移和旋转，以实现点云的匹配。通过预先处理和优化计算，能够在保证匹配精度的同时提高匹配速度。另外，代码中还实现了计算匹配的精度或不确定性矩阵的功能，以及计算多个点云与目标点云的比较结果，统计每组匹配的条件数和比例的功能。

…详情请参照古月居