激光SLAM-基于深度图的闭环检测

激光SLAM-基于深度图的闭环检测

这此一共包含四篇论文，因为他们是同一个作者写的，内容前后关联，层次递进，所以放在一起和大家讨论，四篇论文按照时间顺序分别为：

论文1：Robust On-line Model-based Object Detection from Range Images

论文2：Robust Place Recognition for 3D Range Data based on Point Features

论文3：Point Feature Extraction on 3D Range Scans Taking into Account Object Boundaries

论文4：Place Recognition in 3D Scans Using a Combination of Bag of Words and Point Feature based Relative Pose Estimation

附赠自动驾驶学习资料和量产经验：链接

我们先梳理一下他们的核心点之间的关系。

在基于3D雷达做目标检测的任务中，有一种简便的思路是把雷达转成深度图（range image），这样就把三维空间的处理转成了熟悉的二维图片的处理，以前的图像处理方法就可以借鉴使用了。

论文1就是初步实现了这种思路，它在得到深度图之后，使用Harris方法得到了特征点，然后计算了描述子，使用这种方法对数据集进行处理，可以得到先验特征，当采集到新的激光数据的时候，提取特征和数据集中的数据进行匹配便可以得到检测结果。

论文2把论文1的思路用在了场景识别上，这时候就可以用来做SLAM的闭环检测了，这时候作者认为Harris提取特征用在深度图上还是没有用在真正的图像上时效果好，所以就对特征点提取方法做了改进。

论文3更进一步，对论文1和论文2中所使用的特征点和描述子提取方法都做了改进，在描述子上最大的改进就是增加了旋转不变性。

论文4把论文3的提取方法用在了场景识别上，同时在论文2框架的基础上增加了词袋模型（BOW），提高了匹配搜索时的效率。

下面我们就看看具体的算法。

一、 特征点和描述子

1. 深度图

本系列论文特征提取是基于深度图的，所以我们要先介绍深度图。

对于二维图像的处理我们已经积累了很多成熟的方法来解决我们的问题，当面对使用三维点云数据来处理同样的问题时，事情就发生了变化，我们重新需要研究三维空间的特征及其描述，如果尝试过，就发现这样会使事情变得繁琐和复杂。所以目前很多做法是先把三维点云投影到二维图像上去，图像上的像素和该点的深度直接关联，这样其实并没有损失三维信息，但是却能够使用以前就有的二维图像处理方法，就简化了问题。下面两张图就分别是原始点云和投影后的深度图的例子。

其实不仅传统提取特征点和描述子的方法使用这种思路，即使在端到端的3D目标检测中这种思路也很常见，有的使用俯视图当做图像做卷积，有的同时使用前视图和俯视图，虽然也有直接用三维voxel直接做卷积的方法，但投影的思路是这个领域很重要的一个流派。

2、特征点

聊完了深度图我们聊聊特征点。

在图像中，常见的特征点有SIFT、SURF、Harris、ORB等，这些都是很好的方法，在论文1中作者就是直接使用了Harris方法提取了特征点，但在论文2中就已经意识到这种方法的局限性了，它给出的解释是三维点云投影后分辨率没有图像高，也就是像素之间的距离偏大，所以就自己设计了一种特征点提取方法。

这个方法是基于梯度的。

在上面的式子中，x、y是投影后对应的像素的横纵坐标，r代表的是深度值，也就是说每个像素的梯度又它相邻的像素做差得到。

接着进行了梯度求导

然后进行了二次求导

最后得到了综合指标

通过这个指标，我们就可以找到梯度变化大的地方，也就是可以成为特征的地方。

但是，如果要考虑的全面些，有两个异常情况需要剔除：

1）障碍物

一个物体前面有一个障碍物的时候，它的边缘会被当做特征，随着视角的变化，这些特征其实是变化的，也就是无意义的，作者剔除的依据是这些地方的梯度会比正常特征偏大，所以把梯度变化过大的特征都去掉了。

2）在线上的特征

这个比较好理解，就是我们要提取的是点特征，线特征不好匹配，所以在选取的时候就选取x和y两个方向梯度都比较大的点就可以了。

到了论文3，作者认为原来的方法还不够好，就提出了NARF（normal aligned radial feature）特征提取方法，目的就是尽量减小不同视角造成的影响，提取更稳定的特征点。

这个提取特征点的流程可以用下图表示

图中(a)就是深度图以及提取的边缘，(b)中是显示地展示边缘提取结果，©在20 cm尺度下提取特征点，(d)是在1 m尺度下提取特征点。

在这个提取的过程中，只提取了法向量满足一定要求的特征点，这也就是这种方法名字normal aligned radial feature的由来，这样可以获得更稳定的特征。

论文4就是对论文3这种方法的应用了。

这就是这四篇论文在特征点这一方向上的脉络。

3. 描述子

在论文1中使用Harris进行特征点提取时，作者就是自己设计的描述子计算方法。这一点我每太理解，为什么没有使用图像处理里现有的描述子计算方法。虽然后续特征提取也是自己设计的，但是在最早的论文1里是使用了图像方法里现成的，而描述子上为什么没这样做。

论文1中的描述子计算方法一共分5步：

1）在这整点周围选择一定数量的点

2）计算这些点的法向量，这个法向量可以当做该特征点的法向量

3）沿法向量方向选择一个点v

4）把观测位置定为v，然后观测方向定位法向量的反方向

5）沿着观测方向和选择的特征点周围的点一起计算描述子向量f（具体公式看论文吧）

这个法向量描述方法在论文2中也被沿用了下来，知道论文3中提出NARF方法的时候才被改进。

在论文1中，为了获得尺度不变性，在计算描述子向量时对特征点周围选取的点进行了取平均，但并没有实现旋转不变性，论文3中的改进就是为了解决这个问题。

下图就是描述子计算过程，其中(a)同样是深度图，其中图像正中心椅子右上角的那个加号就是特征点的位置，(b)中红色箭头就是最终计算得到的方向。

4. 特征关联

特征关联这部分就比较简单了，尤其是对视觉slam比较了解的大佬们。

由于有描述子的存在，描述子之间可以计算距离，这个距离体现的就是相似程度，当一帧新的点云到来的时候，从已有点云的特征点中索引与自己相似的帧，就可以计算相对位置了。

在这一环节上，论文1主要是目标检测，所以不需要计算相对位置，只需要查找相似性，论文2是做了位姿计算，并给位姿的准确性打分，打分的方式就是先按照匹配的位姿对其两帧点云，然后计算相应的特征点之间的距离，有点类似于视觉slam里面的重投影误差了，论文4中基于新的特征点提取方法重新做了一遍，区别是添加了词袋模型，在论文2中是逐帧去比较相似性，这样比较耗时间，在论文4中使用词袋模型以后就可以全局搜索了，加快了查找进度。