SSL的个人Blog - SLAM, 回环检测

`Scan Context` 和 `Scan Context++`算法说明

2025-03-01T00:00:00+08:00

1. `Scan Context`

Scan Context论文通过构建旋转不变的PC描述子实现基于点云结构的回环闭合检测功能。

1.1 `PC`描述子构建过程

PC描述子的构建在极坐标下，将点云信息编码成2.5D的描述子信息。以雷达坐标系为中心，在BEV视角下沿着极坐标系的轴向和角度方向划分bin，然后bin中保存着最高点对应的z坐标值，这个过程可以认定为点云将采样过程，其中轴向和角度方向可以由$N_r$和$N_s$两个超参数表示，同时也控制着得到PC描述子的分辨率，PC描述子的构建过程如下图所示，其中蓝色的点为空bin对应的颜色，PC描述子矩阵将其编码为0。PC描述子矩阵中，row编码着下图(a)黄色圈bin，col编码着下图(a)青色bin。使用最高点的高度作为编码的原因是作者认为，最高点可以较大程度的描述一个区域内的特征。

想象一下，当激光雷达的航向角发生变化时，PC描述子矩阵的col可能会发生整体左移，且超出列边界的列会依次填补到最左侧。因此当激光雷达仅发生航向角变化时，我们总可以找到一个列左移数$n$，使得两个点云对应的PC描述子矩阵完全相同，这也恰恰证明了PC描述子的旋转不变性。根据这个逻辑，作者定义了由PC描述子表示的两个点云之间的相关性公式如下。

$$ \begin{align} d(I^{q},I^c)&=1-\frac{1}{N_s}\sum_{j=1}^{N_s}{\frac{c_j^q\cdot c_j^c}{||c_j^q||||c_j^c||}}\\ D(I^q,I^c)&=\min_{n\in [N_s]}{d(I^q,I_n^c)}\\ n^*&=\arg \min_{n\in [N_s]} {d(I^q,I_n^c)} \end{align} $$

式中： - $I^{q}$，当前激光雷达点云对应的PC描述子矩阵； - $I^c$，为数据库中雷达点云对应的PC描述子矩阵； - $I^c_n$，为数据库中雷达点云对应的PC描述子矩阵列左移动n列后得到的PC描述子矩阵； - $d(I^{q},I^c)$，为1-矩阵平均列余弦相似度，作为矩阵距离； - $D(I^q,I^c)$，为当前激光雷达点云描述子$I^{q}$与数据库中某个点云描述子$I^c$的距离； - $n^*$为最小距离对应行左移索引，此外，这个左移索引对应的旋转角度还可以作为$R_{cq}$旋转矩阵的初值。

左移索引与旋转初值的关系

当数据库中的描述子矩阵和当前描述子矩阵为同一地点的不同角时，假设数据库对应描述子沿z轴逆时针旋转k个角度单位时，对应描述子矩阵列必然向左移动n个单位，这是因为世界坐标系下的静态物体在移动过程中，世界坐标不会发生改变，但在雷达坐标系下却向右旋转了而，反应在描述子矩阵上为向左移动的一段距离。这时n乘单位bin对应的角度即可描述旋转矩阵$R_{cq}$的初值。

1.2 `Scan Context`算法思路

从PC描述子的相似度计算公式上来看，要计算当前点云和数据库中点云的相似程度，需要对数据库中的所有PC描述子进行暴力检索，为了提高计算效率，Scan Context算法提出了Ring key描述符来加快回环闭合点云检索过程。Ring Key的计算公式可以由下面的公式描述：

$$ \begin{align} &k_r&=&[\varphi(r_1),..., \varphi(r_{N_r})]^T\\ &\varphi(r_i)&=&\frac{||r_i||_0}{N_s} \end{align} $$

其中，

$r_i$代表PC描述子矩阵第$i$行，代表一圈描述子信息；
$\varphi(r_i)$为PC描述符的行编码公式，由平均L0范数描述，即计算行中非零元素的个数，并除以N_s，得到行编码；
$k_r$为Ring Key，其为所有行得到的计算结果的集合。

通过上面描述的Ring Key计算公式，可以由PC描述子计算得到行编码值组成的Ring Key向量，由行方向上非0块数量近似代表这部分的点云特征。Scan Context使用kdtree存储Ring Key向量，并使用kdtree进行检索，从而提高检索效率。

Scan Context算法的详细流程如下图所示，可以发现Ring Key描述符检索kdtree可以快速得到最相似的PC描述符。然后使用上面给出的PC描述子距离公式计算点云相似程度。

1.3 `Scan Context`源码拓展内容

从Scan Context代码仓库给出的C++代码可以看出，代码中针对PC描述符相似度计算过程做了一定优化。代码中引入了Sector Key行向量描述符来快速计算出$n^*$大小。Sector Key行向量计算过程可由下面公式描述：

$$ \begin{align} &k_s&=&[\phi(c_1),...,\phi(c_{N_s})]\\ &\phi(c_i)&=&\frac{||c_i||_1}{N_r} \end{align} $$

其中，

$c_i$为PC描述子矩阵第$i$列，代表一列描述子信息；
$\phi(c_i)$为Sector Key的列编码公式，由平均L1范数描述，并除以N_r，得到列编码；
$k_s$为Sector Key，其为所有列得到的计算结果的集合。

可以这么理解，Sector Key可以描述以雷达坐标系为中心的径向方向上的点云特征，可以由下面的公式计算两个Sector Key的相似程度，并计算出对应的$n^*$，然后使用$n^*$邻域作为PC描述符相似度的计算区间，可以大大减少$n$的取值范围，从而提升PC描述符的计算效率。

$$ \begin{align} &d(k_s^{q},k_s^c)&=&1-\frac{k_s^q\cdot k_s^c}{||k_s^q||||k_s^c||}\\ &D(k_s^q,k_s^c)&=&\min_{n\in [N_s]}{d(k_s^q,{k_s}_n^c)}\\ &n^*&=&\arg \min_{n\in [N_s]} {d(k_s^q,{k_s}_n^c)} \end{align} $$

其中，

$k_s^{q}$为当前点云的Sector Key向量；
$k_s^c$为数据库中点云的Sector Key向量；
${k_s}_n^c$为左移n列时对应的Sector Key向量；
$d(k_s^{q},k_s^c)$为Sector Key向量距离；
$D(k_s^{q},k_s^c)$为最小Sector Key向量距离；
$n^*$为最小距离对应列左移索引。

2. `Scan Context++`

2.1 `CC`描述符构建

scan context++算法除了使用PC描述符以外，还引入了CC描述符，CC描述符的构建过程如下图所示。与PC描述符构建的极坐标系不同，CC描述符构建的坐标系为笛卡尔坐标系，并以当前雷达坐标系为中心，在给定bin尺寸后，可构建由矩阵描述的Cart Context描述符。

Cart Context描述符的距离计算过程与PC描述符距离计算过程一致，并且Scan Context++算法在论文中提到了Scan Context代码优化中的Sector key，从而加速描述符距离计算过程。

2.2 虚拟描述符增强

在Scan Context算法中，提到了Root Shif的概念，即为了增强PC描述符的平移不变性，将激光雷达点云中心移动到其他位置重新计算描述符，从而得到一个虚拟描述符，并虚拟描述符对应的平移变换，在计算相似度时，除了和原PC描述符计算距离外，还需要和虚拟描述符计算距离，取最小值作为最终的相似度。

Scan Context++算法补充了CC描述符增强概念，为了提高CC描述符的旋转不变性，构造旋转180度的CC虚拟描述符。反映在CC描述符矩阵上为上下和左右倒置。

`FAST-LIVO2重定位和回环思路`

2025-03-01T00:00:00+08:00

背景

在最近的项目开展中，我负责速腾M1激光雷达和森云相机SG2-AR0233实现带有回环和重定位功能的融合SLAM算法开发。我自主改进了Scan Context++算法，并根据最新开源的FAST-LIVO2系统特性，开发了高效的回环闭合和重定位功能，这里对Scan Context++算法的改进过程和适配FAST-LIVO2特性的重定位和回环功能逻辑进行记录，下图是项目使用的传感器平台照片。

修改`Scan Context++`算法

根据我对Scan Context++算法的分析和使用经验发现，Polar Context描述子检测到的回环往往更稳定且更多，而Cart Context描述子由于作用在笛卡尔坐标系中，受到旋转干扰时，Cart Context往往不能检测出回环情况，但Cart Context描述子距离计算往往能得到平移初始值。因此在回环候选寻找阶段，我仅使用PC描述子而不使用CC描述子，这样可以避免由于旋转问题导致CC描述子鲁棒性不高的问题，当PC描述子检测到候选回环时，根据PC描述子给出的旋转初始值估计$R_{cq}$，将当前激光雷达帧旋转到候选雷达帧对应位置后，构造当前帧的Cart Context描述子，然后根据构造的当前帧新CC描述子和回环候选的CC描述子进行匹配计算得到平移估计初始值$t_{cq}$。修改后的算法流程如下。

Bin编码过程分为PC极坐标编码和CC笛卡尔坐标编码；
PC描述子构建过程与Scan Context算法保持一致；
增强PC描述子过程对应Scan Context论文中的Root Shift过程
Ring key计算过程与Scan Context算法保持一致；
数据库kdtree对应的索引由Ring key描述，而内容保存增强PC描述子以及对应相对平移量、CC描述子和位姿；
最近回环候选寻找由kdtree根据Ring Key检索给出；
sector key计算估计初始值过程与Scan Context算法保持一致；
PC描述子计算实际n与Scan Context算法保持一致；
根据PC估计的$R_{cq}$计算去旋转CC描述子和当前状态CC描述子；
row key和col key计算，其实是行方向平均$L_1$范数，列方向平均$L_1$范数计算对应的列向量和行向量，然后使用Scan Context++算法描述的最小描述符距离计算得到最小值对应的行偏移索引和列偏移索引；
使用行偏移索引和列偏移索引计算两个CC描述符对应邻域内的最小值，然后根据CC描述子的分辨率获得估计的平移向量。而初始的旋转角度由PC描述子距离计算给出。

`FAST-LIVO2`回环闭合功能和重定位功能

我使用子地图策略实现FAST-LIVO2的回环闭合和全局重定位功能。上图描述的是回环闭合和重定位功能主要包含以下几个方面：

子地图结构和保存逻辑（保存时机、保存内容）
子地图之间的点云配准和点云与子地图之间的配准逻辑
回环闭合功能开始逻辑，对于回环闭合功能，当检测到的回环和当前帧之间不在同一子地图时，回环闭合功能开始
全局重定位功能逻辑，全局重定位逻辑主要是在已有子地图保存文件基础上，系统开始时做的重定位逻辑，确定当前帧与回环子地图之间的相对位姿
子地图状态更新逻辑，当子地图之间位姿图优化完成后，需要将每个子地图内部对应的点云帧位姿进行更新、平面点更新、平面法向量更新，基于位姿变换更新，更新逻辑比较简单。

子地图结构和保存逻辑

下面图是我截取的FAST-LIVO2的局部地图的管理逻辑，其中区域A和区域B组成了上一时刻的局部地图，区域B和区域C组成这一时刻的局部地图。在区域A析构前，需要对区域A和区域B组成的之前的局部地图进行磁盘永久保存。主要保存内容有：

体素内平面中心点，用于子地图之间的配准；
体素内平面法向量，用于子地图之间的配准；
局部地图内包含的点云帧id信息和位姿信息，用于scan context++构造的点云之间位姿初值向子地图之间位姿转换；
局部地图内包含的点云帧对应的PC描述子、PC增强描述子、CC描述子，用于当前点云帧的scan context++候选回环检测；

子地图之间的配准逻辑和点云与子地图配准逻辑

当回环闭合开始时，当使用改进的scan context++算法获取数据库回环候选的点云帧后，根据改进的scan context++算法，可以拿到初始的描述当前点云帧和回环候选点云帧之间的旋转矩阵和平移向量，将这部分相对位姿转换到对应的两个子地图下。使用双向映射的点到面的ICP算法，实现子地图之间的配准逻辑。

当SLAM系统开始时，需要寻找当前点云在之前地图中的位姿信息，当使用改进scan context++算法获取到候选回环点云后，加载子地图并构建点到面的ICP问题，并使用huber核函数估计当前帧与候选回环子地图的相对位姿关系。

SSL的个人Blog - SLAM, 回环检测

`Scan Context` 和 `Scan Context++`算法说明

1. Scan Context

1.1 PC描述子构建过程

1.2 Scan Context算法思路

1.3 Scan Context源码拓展内容

2. Scan Context++

2.1 CC描述符构建