Abstract
提出一个新的词袋,用于在3D Lidar中实时检测回环。方法不仅有效地识别重新访问的回环位置,而且还实时校正完整的6-DoF回环姿态。
基于3D雷达特征 Link3D 构建词袋该特征高效、姿态不变,可用于精确的点到点匹配。
在大多数情况下,BoW3D在F1最大值和扩展精度分数方面表现更好。BoW3D在配备Intel Core i7@2.2 GHz处理器的笔记本电脑上执行时,识别和纠正KITTI 00上的环路(包括4K+64光线激光雷达扫描)平均需要48毫秒。
开源
Contributions
数据结构:哈希表
找到候选帧后,我们使用RANSAC[12]和SVD[13]的精确点到点的LinK3D匹配结果来计算全6-DoF环路姿态。
嵌入ALOAM评估表现
- 一种新的基于BOW的方法,为雷达slam提供闭环检测。建立了LinK3D特征数据库,并实时有效地识别重新访问的地点。
- 可以用于实时校正全6-DoF闭环姿态,这为在线操作中的后续姿态图优化提供了精确的环路闭合约束。
- 该方法已在实际应用中嵌入到激光雷达里程计系统中。实验结果表明,该方法可以显著消除漂移,提高三维激光雷达SLAM的精度
并未提及鲁棒性
背景
LinK3D:
由三部分组成:关键点提取、描述符生成、特征匹配。
LinK3D描述符的核心思想是使用邻域信息来表示当前关键点。LinK3D描述符由180维向量表示。描述符的每个维度对应于一个扇形区域。第一个维度对应于当前关键点的最近关键点所在的扇区区域,其他维度对应于按逆时针顺序排列的区域。如果扇区区域中存在关键点,则搜索扇区区域中最近的关键点,并将其用于表示描述符的相应维度。
轻量级,在使用Intel Core i7@2.2 GHz处理器的笔记本电脑上执行时,从64射线激光束收集的点云中提取特征平均需要32毫秒。
可以用来实现点到点匹配,可以运用于快速的3D配准
词袋:
在2D图像领域,通过检索2D特征(如SIFT[24]、ORB[25]等),使用BoW来识别重新访问的地点。BoW在从训练图像数据集提取的一组描述符上,用离线步骤创建一个作为树结构的视觉词汇表。当处理新图像时,词汇表将提取的图像特征转换为低维向量,包含term frequency和inverse document frequency(tf idf)[9]分数。
其中$n_{wi}$表示图像$I_i$中的单词w的数量,$n_i$表示图像$I_i$中的单词总数,N表示迄今为止看到的图像总数,$n_w$表示包含单词w的图像的数量。
idf的想法是,单词在所有图像中出现的次数越少,最终得分就越高,这表明该单词具有更高的辨别力。tf idf总分越高,该词在图像中的频率越高。如果单词的得分足够高,则计算该单词与数据库中的单词之间的相似度。如果数据库中有类似的单词,则使用反转索引来搜索相应的图像。
方法
首先从原始点云中提取Link3D特征点,然后用里程计和建图算法估计机器人位姿。第三部分使用所提出的BoW3D来回环检测并校正回环姿态。如果检测到并优化了回环,环路闭合将向映射算法提供反馈并更新局部映射,这为后续步骤提供了更准确的估计。
A. BoW3D Algorithm
不需要加载额外的词典文件。哈希表用于在单词和单词出现的位置之间建立一对一的映射。时间复杂度理论上O(1)。
BoW3D词汇表中的单词由两部分构成:一部分是LinK3D的非零维度;另一个是单词所在的对应维度。地点(点云帧)也由两部分组成:一部分是帧Id;另一个是帧中的描述符ID。
place set 记录了word出现在第几帧中的第几个描述子
1.Retrieval Algorithm
文中的检索算法继承了tf-idf的思想。对于LinK3D描述符的单词,我们检索单词并计算每个位置(点云帧)出现的频率。如果最高值大于频率阈值$Th_i$,则该位置被视为候选位置。此外,为了快速检索,使用反向文档频率(idf)避免检索出现在多个位置的单词。更具体地说,由于这些词明显比其他词出现的频率更高,它们的区分性较差,并且会降低检索效率。因此,我们定义了一个类似于idf的比率因子,以测量当前集合中的位置数($N_{set}$)与所有集合中的平均值之间的差异。我们使用它来确定在计算位置数时是否应该保留当前单词的位置集。比率定义如下:
2.Loop Correction:
回环矫正在后端给位姿图优化提供约束。首先根据LinK3D的匹配结果计算回路的观测约束,然后使用RANSAC消除误匹配。计算回环位姿$T_{l,c}$ ,即回环帧l和当前帧c的估计(相对姿态)
给定当前帧的点集${S}_c$和回环帧的匹配点集$S_l$ ,我们通过最小化以下cost function来计算循环:
去质心,SVD分解,得到R,t(同ICP中找到最近点之后求解最优变换的步骤)
3.Update Algorithm
作用:向数据库中添加新word和place。为了提高更新和检索的效率,只向数据库中添加一定数量的特征。描述符基于其对应关键点与LiDAR中心之间的距离来选择,只有一定数量的更接近的特征被添加到数据库中。类似地,当检索循环时,使用一定数量的更接近的特征来检索。
B. 回环优化
在校正循环位姿之后,构建循环帧的位姿图以用于后续的循环优化。位姿图的顶点是要优化的全局位姿。位姿图的边是观察的约束,该约束由相邻帧之间的相对位姿和回环位姿组成。
定义残差模型:
最小化cost function优化位姿图:
S: 所有连续边的集合;L:所有回环边的集合。使用g2o优化器通过LM方法进行优化。
位姿优化后更新建图线程的局部地图。用优化的全局姿态进行更新,确保后续估计的全局一致性
实验
四个方面评估:(i) 评估位置识别表现;(ii)评估此方法集成到3D激光雷达SLAM中时的性能;(iii)测试超参数并分析我们方法的鲁棒性;(iv)测试系统各部分的运行时间
准确率,召回率?
KITTI数据集
(i)
与最先进的LiDAR环路闭合检测和位置识别方法进行了比较,包括M2DP[28]、Scan context[29]、Point-NetVLAD[30]、ntensity Scan Contex(ISC)[32]、LiDAR Iris[33]、SGPR[34]、OverlapNet[35]和SSC[36]。
F1 && EP
F1表示精度和召回的调和平均值,它将精度和召回视为同等重要,并用于评估分类的总体性能。
其中PR0表示最小召回时的精度,RP100表示100%精度时的最大召回。EP专门设计用于验证位置识别性能。
(ii)
(i) Euclidean distance for translation; (ii) Θ for rotation;(iii) RMSE
A-LOAM
(iii)