上一篇聊HDL-Graph-SLAM的时候末尾提到了回环检测。回环检测是SLAM系统中消除累积误差的关键一环。机器人走了一圈回到起点如果系统认不出我来过这里那轨迹就会漂移地图也会错位。这一篇我们深入聊聊回环检测的核心技术重点放在词袋模型和位置识别上。回环检测的本质是一个场景识别问题。给定当前帧的观测数据判断机器人是否曾经到过这个位置。听起来简单但实际挑战很多视角变化、光照变化、动态物体遮挡、感知混叠不同地方看起来很相似这些问题都会影响检测的准确性。词袋模型BoW的基本原理词袋模型是回环检测最经典的方案源自文本检索领域。核心思路是把图像表示成一组视觉单词的直方图然后通过比较直方图的相似度来判断两帧图像是否来自同一个地方。整个流程分三步。第一步是构建词典vocabulary。从大量图像中提取特征描述子比如ORB、SIFT用K-Means聚类把这些描述子聚成K个类每个类的聚类中心就是一个视觉单词。所有视觉单词组成词典。# 词袋模型的核心流程 features [extract_orb(img) for img in training_images] all_descriptors np.vstack(features) # K-Means聚类构建词典 vocabulary kmeans(all_descriptors, k10000)第二步是把每帧图像转换成词袋向量。提取当前帧的特征描述子把每个描述子匹配到词典中最近的视觉单词统计每个单词出现的次数或TF-IDF权重得到一个K维向量。第三步是比较词袋向量的相似度。常用的度量方式有余弦相似度、卡方距离等。相似度超过阈值就认为检测到了回环。DBoW2是目前用得最多的词袋库ORB-SLAM、VINS-Mono等主流系统都用它。DBoW2用了分层聚类来加速匹配把词典组织成一棵二叉树或四叉树查询时间从O(N)降到O(logN)实时性很好。TF-IDF权重和相似度计算直接用单词出现次数做相似度比较有问题——有些视觉单词在所有图像中都频繁出现比如天空、墙壁这些常见词对区分不同场景没什么帮助。TF-IDF就是用来解决这个问题的。TF词频衡量一个单词在当前图像中出现的频率。IDF逆文档频率衡量一个单词在所有图像中有多稀有。一个单词越稀有IDF越大权重越高。# TF-IDF权重计算 def tf_idf_weight(tf, df, n_docs): idf math.log(n_docs / (df 1)) return tf * idf两帧图像的词袋向量算好TF-IDF权重后用L1范数或L2范数计算相似度。DBoW2默认用L1范数归一化到[0,1]区间。相似度大于0.8通常认为是很强的回环候选。感知混叠和时序约束回环检测有两个典型问题。一个是感知混叠perceptual aliasing两个不同的地方看起来很像比如两条相似的走廊。这会导致误检false positive。另一个是感知变化perceptual variability同一个地方在不同条件下看起来不同比如白天和黑夜。这会导致漏检false negative。处理感知混叠常用的方法是加几何验证。词袋模型给出的只是外观相似度检测到回环候选后再用特征匹配计算相对位姿检查几何上是否一致。如果匹配的内点数不够多或者计算出的相对位姿和里程计估计差太大就拒绝这个回环。# 几何验证流程 def verify_loop(query, candidate): matches match_features(query, candidate) if len(matches) min_inliers: return False T estimate_pose(matches) if pose_error(T, odometry_estimate) threshold: return False return True处理时序约束可以利用时间一致性的先验。如果第i帧和第j帧之间有回环那第i1帧和第j1帧之间也很可能有回环。ORB-SLAM用了一个回环候选组的机制连续多帧都检测到同一个候选才确认回环大大降低了误检率。3D点云的回环检测词袋模型主要用于视觉SLAM。对于3D激光雷达SLAM回环检测的思路不太一样。Scan Context是一种很流行的3D点云回环检测方法。它把点云投影到一个极坐标BIN矩阵中每个BIN存储该区域内点的最大高度。这样就得到一个2D描述子可以快速比较两帧点云的相似度。# Scan Context的核心思想 def scan_context(points, num_rings20, num_sectors60): sc np.zeros((num_rings, num_sectors)) for p in points: r, theta to_polar(p) ring int(r / max_range * num_rings) sector int(theta / (2*np.pi) * num_sectors) sc[ring][sector] max(sc[ring][sector], p.z) return scScan Context的优点是计算快、对旋转不敏感可以通过列偏移来处理旋转。缺点是描述能力有限对视角变化大的场景效果不太好。后来还出现了M2DP、LiDAR Iris等改进方法描述能力更强。最近的趋势是用深度学习来学点云描述子比如OverlapNet、LPD-Net精度更高但计算量也更大。面试追问环节面试官词袋模型的词典大小怎么选词典大小是个trade-off。词典太小描述能力不够不同场景的词袋向量可能很相似导致误检。词典太大很多视觉单词只在少数图像中出现词袋向量太稀疏相似度计算不稳定。经验值是1万到10万ORB-SLAM2用的是10万的词典。具体要根据场景复杂度来调。面试官DBoW2的分层聚类怎么加速的DBoW2把词典组织成一棵K叉树。每个节点存储一个视觉单词的聚类中心。查询时从根节点开始每一层只走最近的分支不需要遍历所有单词。这样查询复杂度从O(N)降到O(L*K)L是树的层数K是分支因子。通常4层、10分支总共1万个叶子节点查询速度快很多。面试官回环检测误检了怎么办误检的后果很严重——会把一条错误的约束加入位姿图导致地图扭曲。处理方式有几层防线。第一层是几何验证检查匹配点的几何一致性。第二层是时间一致性要求连续多帧都检测到同一个候选。第三层是在优化阶段检查如果加入回环后优化误差突然增大就剔除这个回环。ORB-SLAM还有Essential Graph来传播回环校正避免单条错误回环破坏全局地图。面试官Scan Context和词袋模型有什么区别最大的区别是数据模态不同。词袋模型处理2D图像特征Scan Context处理3D点云。词袋模型需要训练词典Scan Context不需要训练是手工设计的描述子。词袋模型的描述维度是词典大小通常10万Scan Context的描述维度是RING数乘以SECTOR数通常1200。在激光SLAM中用Scan Context在视觉SLAM中用DBoW2多传感器系统两个都可以用。面试官动态物体对回环检测有什么影响影响很大。比如上次经过一个地方停了一辆车这次经过车开走了场景变化很大词袋向量差异就大可能漏检。反过来如果动态物体很多比如行人不同地方可能因为都有人而看起来相似导致误检。处理方法是在提取特征前先做动态物体检测和剔除只用静态物体的特征来做回环检测。回环检测是SLAM系统的记忆模块让机器人能够识别曾经到过的地方。词袋模型用视觉单词的统计信息做场景匹配Scan Context用点云的极坐标投影做位置识别。两者思路不同但目的一样找到回环消除累积误差。好的回环检测系统不是单一算法而是多层防线的组合——外观匹配给出候选几何验证排除误检时间一致性增强可靠性。上一篇第253篇 HDL-Graph-SLAM深度解析下一篇我们聊ICP点云配准这是回环检测和很多3D SLAM系统中的核心算法。如果这篇文章对你有帮助欢迎点赞支持一下你的鼓励是我持续更新的动力
