1.简介
Scan Context 就包括空间描述子的定义方法和与之对应的匹配算法。并提供了高效的 bin 编码函数,同时这种编码对点云的密度和法向的变化不敏感。另外SC还保存点云的内部结构,并使用一种有效的两阶段匹配算法,相比于其他空间描述子而言性能更好。
2.论文浅读
基于 Scan Context 的位置识别方案整体流程如下图所示:
Scan Context,从英文字面理解就是“ 扫描 上下文 ”。类比于我们阅读的时候,需要理解上下文,才能明白其意,LidarSLAM在进行回环检测的时候,也需要将“上下文” (之前的数据)进行比较,方才知道我们是不是又走到了之前的同一个地方( 回环 )。
2.1提出方法
对一帧点云先进行 SC(Scan Context)的计算;
从该帧点云的 SC 中提取出一个 N 维的向量(和环数一致)用于在 KD 树中搜索相近的关键帧;
将搜索得到的参考帧的 SC 和待匹配的当前帧进行比较,如果比较得分高于一定阈值则认为找到回环。
3.主要流程
3.1 scan context的创建
(1) 与Shape Context的渊源
Scan Context这个算法其实一开始是由Shape Context [2] 所启发的,而Shape Context是把点云的 local Keypoint 附近的点云形状 encode 进一个图像中。
Scan Context的不同在于,它不仅仅是count the number of points,而是采用了 maximum height of points in each bin(简单来说,就是取每一个bin中的所有point的z轴最高点的value作为这个bin的value)。
(2) 为什么选择Maximum height?
a.使用高度的原因是为了有效地总结周围结构的垂直形状。
b.此外,最大高度表示从传感器可以看到周围结构的哪一部分。
c.这种以自我为中心的可见性在城市设计文献中是一个众所周知的概念,用于分析一个地方的身份
(3) 点云划分
将一帧 3D 点云按照传感器坐标系中的方位角和半径均匀划分不同的 bin,如上图所示,从方位角上看点云被划分成 $N_s$ 个扇面(Sector),从半径反向看,点云被划分成 $N_r$个环(Rings),每个扇面和环相交的部分为一个 bin。每个扇面和环的宽度(分辨率)可以从点云的最大检测距离和扇面/环数量计算得到。
从这种划分方式不难发现,距离较远的 Bin 相比于距离较近的 Bin 会稍微宽一点。这样划分的好处是可以自动对不同距离的点云密度进行动态调节。对于距离较近的地方通常点云密度会高一点,因此我们的 Bin 取窄一点,而相反,对于较远的地方,点云会比较稀疏。因此 Bin 取宽一点可以容纳更多点。
(4)给每个Bin进行赋值:Bin Encoding
给每个 Bin 分配一个数作为标识,SC 中用每个 Bin 的点中最大的高度作为该 Bin 的标识,对于没有任何点的 Bin 则用 0 作为标识。即:
公式解读:
就是指属于第i个环和第j个扇区重叠的bin的点的集合s
z(⋅)是指中一个point的z坐标
直接使用最大z坐标值 z(p),作为这个bin的value。
从上图中不难看到,虽然 SC 在空间上来看是一个圆,应该有旋转不变性。但我们按照 0-2pi展开成 2D 图像后,SC 对 Lidar 的朝向就变得敏感了。朝向稍微偏几度可能整体图像就有比较大的平移从而不相似了,为了解决这个问题,作者对图像进行 $N_{trans} = 8$ 次平移,通过这种方法来包含 Lidar 在不同朝向下的 SC。
(5)scan context矩阵
scan contextI最终表示为Nr×Ns矩阵,如下所示:
3.2 相似度计算
假设我们得到了一对Scan Context的矩阵,我们要计算他们俩之间的相似度,文章中采用了columnwise (按列) 的距离计算
:Query Point Cloud (简言之,我们当前用来query的点云)。
:Candidate Point Cloud (咱们的“数据库”中储存的用来匹配的candidate点云)。
:Column j of Query Point Cloud (列向量)。
:Column j of Candidate Point Cloud (列向量)。
那么问题来了:这样来比较两个点云,而没考虑每次不可能准确的都在同一个位置和角度观察。是不是太理想化了呢?
so:假设我们回到同一个地方,那有可能是沿着相反的方向回来的,那咱们的Viewpoint就发生了变化,这个Scan Context矩阵就会发生偏移! 这样就会导致Column顺序发生变化。所幸的是只要定位是在同一个地方,不管你的方向朝着哪里,至少row order不会发生太大变化。我们只需要关心平移的问题。如下图所示:
可以看出在column方向发生了水平位移,但是竖着的row方向没有变化。
为了解决这个问题,文中使用的方法是不断尝试各种角度的column shift。注意的是,旋转candidate point cloud有个resolution,那就是之前提到的。
使用公式(7)进行最佳shift的选择,找到最好的 n∗后,用公式(6)进行distance计算。
注意:这里咱们通过找最好的 n∗,还有一个意想不到的好处,那就是可以给ICP提供一个Good initial rotation value! (就是ICP代码中的predicted pose)
3.3 两步搜索算法
文中提到,有三种主流的位置识别的Search Algorithm:
- 相似度分数
- 近邻搜索
- 稀疏性优化
文中将计算相似得分和最近邻搜索结合成一步,有效降低了搜索时间。
(1)Ring Key
在3.2节中我们提到的公式(6)进行最短距离计算时,要先找到最佳旋转 n∗ ,计算量很大,所以在本文中提出了一种" Two-phase Search ",并提出了 Ring key 这个Descriptor(描述子)来进行匹配搜索:
环形键是一个旋转不变的描述符,它是从扫描上下文中提取的。扫描上下文r的每一行都通过环形编码函数编码为单个实数。矢量k的第一个元素来自离传感器最近的圆,下面的元素按顺序来自下一个环,如图4所示
由内而外,一圈一圈的ring key通过对Scan Context Matrix的每一行row r 进行ψ ( ⋅ )的encoding就变成了一个N r 维度的Vector k:
环形编码函数ψ是使用L0范数的占有率:
上式的ro就是L0norm(范数)的意思,比如(1,0,2,3)这个vector的范数就是3,因为它有三个非零数。
这样一来,统计每一圈的row中有多少个非零数值,这就和旋转没啥关系了,也就是旋转不变性。
这样就可以达到快速的搜索。
(2)KD-Tree
- 在得到向量k之后,用这个k构建kd树
- 用ring key of the query(也就是当前点云的ring key)到这个kd树中搜索k个最相似的scan索引。
- 得到最相似的k个scan后,用上式(6)进行相似度计算。
- 满足条件的最近的候选对象c∗这个位置被选为重新访问的位置,也就是loop的地方。
4.延伸学习
使用scan context进行ICP初始化。
sc_lego_loam;sc_lio_sam。
基于scan context的重定位。