对于SLAM来说，回环检测是重要的一步，我们知道，当机器人做一个回环时，可能会出现已经达到达起始点，但是由于漂移，位姿估计值却显示当前位置和初始位置还有相差一段距离，这时候，我们就依赖回环检测，把当前位置拉回初始位置，这样就消除了漂移，这个过程就是回环检测的目的。

所以回环检测的关键是，对于某一帧点云来说，它经过一个历史状态之后，我们如何去索引当前帧是否在历史帧中出现过，即检测有没有发生回环。当然，最容易想到的也是最朴素的做法就是，将这一帧的3D点云与历史所有帧做匹配，找到最接近的那一个。但是对于3D点云来这么做计算量无疑是巨大的，而且三维匹配还存在一个初值的问题。

因此，作者提出了ScanContext，上面的动画可以看出ScanContext的工作过程就是把3D点云通过俯视图的方式转换为2D，然后用过2D匹配找到回环，达到回环检测的目的，这样将三维降为两维进行匹配大大减小了计算量，并且它对初值相对不敏感。

ScanContext的主要流程如下：

点云切割

首先是进行点云分割，如下图所示:

首先使用来自 3D 扫描的点云的俯视图，以扫描的中心充当全局关键点。对点云的俯视图进行圆环分割和扇区分割，可以看出以扫描中心为关键点，向外扩散形成$N_r$个同心圆，我们称之为$N_r$个环。然后以中心为关键点对圆环的$[0,2\pi]$进行均匀分割，得到$N_s$ 个扇区。论文中取 $N_s= 60$ 和 $N_r = 20$。

可以发现，沿着半径增大的方向（绿色箭头的方向），把点云分割成了$N_r$个圆环，每个圆环的宽度为：

$$ d_r=\frac{L_{max} }{N_r}\tag{1} $$

其中$L_{max}$表示激光雷达测得点云的最远距离。

通过图不难发现，我们一共得到$N_r$个环和$N_s$ 个扇区，每个环都与扇区有重叠区域，这个重叠区域称之为bin，而且远离传感器的 bin 的物理面积比近的 bin 更宽。设$\mathcal{P}_{ij}$是属于第$ i $个环和第 $j$ 个扇区重叠的 bin 的点集。设激光点经过分割后的集合为$\mathcal{P}$，则：

$$ \mathcal{P}=\underset{i\in[N_r],j\in[N_s]}{\cup}\mathcal{P}_{ij}\tag{2} $$

其中，符号 $[N_s] =\{1, 2, ..., N_{s-1}, N_s\},[N_r] =\{1, 2, ..., N_{r-1}, N_r\}$。

生成ScanContext

经过分割后，我们发现分割后的点云的集合就是bin的集合，而bin是$N_r$个环和$N_s$ 个扇区的重叠区域，每个bin区域可能会有点云也可能没有点云（图（b）中的蓝色像素），而我们将bin的集合表示为矩阵形式，就可以得到一个$N_r\times N_s$的矩阵，其中矩阵的每一行代表一个圆环，则一共有$N_r$行，所以下图纵坐标范围为$[0,L_{max}]$；矩阵的每一列代表一个扇区，则一共有$N_s$列，所以下图横坐标范围为$[0,2\pi]$。

那么bin的值如何确定呢？我们知道，没有点云的bin，我们对其赋值为0，图中用蓝色像素表示；那么有点云的bin如何表示呢？作者在这里使用的是最大高度。

设某个点云为$p$， $z(\cdot ) $是返回点 $p$ 的$ z $坐标值的函数，则任一个点云集合bin的值为：

$$ \phi(\mathcal{P}_{ij})=\underset{p\in\mathcal{P}_{ij}}{\max}z(p)\tag{3} $$

这么做有什么好处？我们知道，在开始我们把三维点云降为二维，损失了高度信息，而这里对高度信息相当于一次补偿作用，保留了信息的完整性。

基于ScanContext的匹配

在得到ScanContext数据之后，我们如何利用ScanContext进行匹配，进而完成回环检测，这是ScanContext最终的目的。

设

• $I^q$为当前帧的ScanContext， $c^q_j $为$I^q$中的第$j$列；
• $ I^c $为历史帧的ScanContext，$c^c_j $为$I^c$中的第$j$列；

两帧之间的距离函数定义为：

$$ d(I^q,I^c)=\frac{1}{N_s}\sum^{N_s}_{j=1}(1-\frac{c^q_j\cdot c^c_j}{\parallel c^q_j \parallel \parallel c^c_j \parallel}).\tag{4} $$

通过这个距离函数可以发现，它反应了两帧之间的差异性，对于相似的两帧，它们的相同列向量的内积的模应该为1，即$\frac{c^q_j\cdot c^c_j}{\parallel c^q_j \parallel \parallel c^c_j \parallel}=1$，则距离接近为0，当然这仅仅是理想情况下，实际上，我们通过设置阈值的方式，当距离函数小于某一个阈值时，认为该历史帧为回环检测的回环帧。

但是这里存在一个问题，就是历史帧有旋转的时候：

这里我们定义的是当前帧与历史帧的相同列进行比较，这个条件是十分强的，假设激光雷达在经过一个地方的时候方向与之前的方向恰恰相反，或者该地方为一个十字路口，激光雷达从不同的方向经过一个地方，则雷达相对于全局坐标的坐标会发生变化，导致得到的ScanContext中的列向量顺序可能发生变化，进而使得两帧的距离函数比较大，导致经过相同地方的两帧点云的距离函数非常大，最终回环检测匹配失败。

如下图所示，这是经过相同地方的两帧点云，它们由于激光雷达的朝向发生了改变，导致两者的ScanContext中的列向量顺序发生变化，距离函数值随之变得非常大。

不难发现，将图(b)后半部分切割放到前面，如下图所示，就可以得到与(a)十分相似的ScanContext.

对于人而言，上述规律是非常容易发现的。但是实际程序中怎么切割，怎么移动才能达到上述效果？

最简单也是最朴素的做法就是，将历史帧$I^c$按列平移，得到$[N_s]$个ScanContext，依次与当前帧的ScanContext计算距离，选择距离最小的，即为闭环匹配的帧。

但是这种方法的计算量也是相对较大的，作者在代码中对此方法进行了优化，优化方法就是将当前帧与历史帧对应列分别求列和，然后根据列和我们找到可能的最佳切割点，但是由于我们降维之后会损失一部分信息，因此我们寻找的切割点并不是足够准确的，所以我们将在切割点附近对对应的二维$m$列依次进行切割平移，得到远远小于$n(n<<[N_s])$个的ScanContext，依次与当前帧的ScanContext计算距离，选择距离最小的，即为闭环匹配的帧。

计算相对位姿

设$I^c_n$ 是一个$\mathrm{Scan Context}$，它的第$n$列是从原始的$\mathrm{Scan Context}$偏移过来的。由于列向量代表的是分辨率为$\frac{2\pi}{N_s}$的扇区，所以每个列对应的角度为$\frac{2\pi}{N_s}$。整个过程，把历史帧切割，然后进行列平移之后，得到与当前帧的良好匹配， 良好的匹配意味着我们通过分割平移将历史帧对应的位姿旋转到当前帧对应位姿的朝向，那么，反过来，我们可以根据平移来求得旋转的角度。

假设距离最小时，对应的列的平移量为：

$$ n^*=\underset{n\in[N_s]}{\arg\min} \ d(I^q,I^c_n)\tag{5} $$

因此，历史帧与当前帧之间的旋转的角度为：

$$ \phi=\frac{2\pi}{N_s}\times n^*\tag{6} $$

这个旋转分辨率在作者代码中为$\frac{2\pi}{N_s}=\frac{2\pi}{60}=6°$，所以匹配的精确度小于$6°$，这并不是一个精确的值，需要进一步精确的话则需要使用ICP或者NDT匹配。因此可以作为ICP或者NDT匹配的初始位姿，用于精确匹配得到闭环约束相对位姿。

解决时间复杂度问题

目的：ScanContext可以使用矩阵对应列的相似度来计算两帧的相似性，但是遍历所有历史帧的相似度计算量比较高，需要做一个快速初步筛选。

思路：相似帧之间，落在同等半径的圆环中的点的数量应该相似，可以用来快速查找。

比如，某一帧中，在圆环$r_i,i\in[N_r]$中，被$[N_s]$个扇区分割为$[N_s]$个bin，对于相似的两帧，对应圆环中，含有点云的bin占有率应该相同，如公式$(8)$。通过这种方法可以进行第一阶段的粗略匹配。

方法：

这里作者引入了一个$ring\ key$的概念。

（1）每一帧生成一个向量$k$

$ring\ key$实质上就是$\mathrm{Scan Context}$的每一行$r$经过编码函数$\psi$编码成一个实数值后，$N_r$个圆环组成了一个$N_r$维向量的$ring\ key$，即为

$$ k=(\psi(r_1),...,\psi(r_{N_r})),\mathrm{where}\ \psi:r_i\rightarrow\mathbb{R}\tag{7} $$

其中，我们使用的环编码函数$\psi$是使用 $L_0$范数的环的占用率：

$$ \psi(r_i)=\frac{\parallel r_i \parallel_0}{N_s}\tag{8} $$

$\parallel r_i \parallel_0$表示半径$r_i$对应的圆环中非空分割单元的个数。

（2）根据（1）中计算的向量，所有历史帧共同构建KDtree;

（3）使用当前帧对应的向量，在KDtree中查找，找到$n$个可能的相似帧，作为候选索引$\mathcal{C}$ 。

（4）使用$\mathrm{Scan Context}$进行第二阶段的精确匹配

虽然不如$\mathrm{Scan Context}$信息丰富，但$ring\ key$支持快速搜索，以找到$n$个可能的候选闭环。使用距离公式将这些恒定数量的候选$\mathrm{Scan Context}$与要查询的$\mathrm{Scan Context}$进行比较。最接近的满足给定阈值$\tau$ 的候选项被选为重新访问的位置：

$$ c^*=\underset{c_k\in\mathcal{C} }{\arg\min}\ D(I^q,I^c),s.t. \ D<\tau\tag{9} $$

其中 $\mathcal{C}$ 是从 KD 树中提取的一组候选索引，$\tau$ 是给定的接受阈值。 $c^∗ $是确定为闭环的位置的索引。