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

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描述符矩阵上为上下和左右倒置。

孙善路-github 对SLAM和DL感兴趣的理工男

孙善路-bilibili 对SLAM和DL感兴趣的理工男

有什么问题吗，有任何问题欢迎你在下面评论留言或者邮件联系我！

Comments

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

1. `Scan Context`

1.1 `PC`描述子构建过程

1.2 `Scan Context`算法思路

1.3 `Scan Context`源码拓展内容

2. `Scan Context++`

2.1 `CC`描述符构建

2.2 虚拟描述符增强

Comments

Keep Reading

Published

Category

Tags

Contact Me

1. Scan Context

1.1 PC描述子构建过程

1.2 Scan Context算法思路

1.3 Scan Context源码拓展内容

2. Scan Context++

2.1 CC描述符构建

2.2 虚拟描述符增强

Comments

Keep Reading

Published

Category

Tags

Contact Me

1. `Scan Context`

1.1 `PC`描述子构建过程

1.2 `Scan Context`算法思路

1.3 `Scan Context`源码拓展内容

2. `Scan Context++`

2.1 `CC`描述符构建