专栏系列2D/3D/视觉SLAM理论详解共10篇 |难度中级 |预计阅读24分钟前置知识特征描述子第4章、图优化/位姿图优化第5章本文定位第7篇详解三种SLAM中回环检测的完整理论——从视觉词袋到激光扫描匹配摘要回环检测是消除SLAM累积漂移的关键机制。本文对比三种SLAM的回环检测策略(1) 2D激光的分支定界扫描匹配Cartographer——在SE(2)空间中全局搜索最优位姿(2) 3D激光的Scan Context——将点云编码到环形矩阵后用距离度量检索(3) 视觉的词袋模型——将图像量化为视觉词汇分布后用TF-IDF检索。三者的本质统一都在将高维传感器数据映射到低维描述空间进行高效的近似最近邻检索。目录1. 回环检测的核心作用与统一框架2. 2D激光分支定界扫描匹配3. 3D激光Scan Context与几何检索4. 视觉词袋模型5. 几何验证三种SLAM的共性6. 位姿图优化与全局一致性1. 回环检测的核心作用与统一框架1.1 为什么需要回环SLAM的前端是增量式的每步仅依赖最近几帧。即使每步误差极小激光0.1° 1cm视觉0.5° 5cm在1000帧后累积误差也分别可达几十度和几米。这种累加是系统性的——不依赖任何外部校正。回环检测通过识别当前位置是之前访问过的地方在时间上相距遥远的帧之间建立约束——将开环轨迹闭合成环。1.2 统一的回环检测流程三种SLAM的回环检测都遵循相同的管道新帧 → 特征提取 → 描述子/编码 → 检索相似帧 → 候选排序 ↓ 几何验证对极/RANSAC/ICP ↓ 相对位姿计算 ↓ 位姿图优化闭环修正步骤2D激光3D激光视觉特征提取扫描点扫描线/点云ORB/SIFT关键点描述子距离变换图Scan Context矩阵256维ORB描述子检索方式分支定界搜索3D位姿空间环形矩阵距离KD树词袋向量倒排索引搜索空间SE(2)x , y , θ x, y, \thetax,y,θ偏航角 平移图像外观空间几何验证ICP精配准ICP/NDT精配准对极几何RANSAC2. 2D激光分支定界扫描匹配2.1 Cartographer的回环检测管道Cartographer使用**分支定界加速的相关扫描匹配CSM with BB**进行回环检测构建似然场对当前子图构建距离变换图每个栅格存储到最近障碍物的距离在SE(2)搜索窗口中枚举候选位姿Δ x × Δ y × Δ θ \Delta x \times \Delta y \times \Delta\thetaΔx×Δy×Δθ每个候选位姿的得分 将当前扫描投影到该位姿后所有扫描点落在似然场中的似然之和最高得分的候选位姿 回环约束的初始估计ICP精配准以上一步为初值做ICP落在精确的SE(2)回环位姿2.2 分支定界Branch-and-Bound穷举搜索的复杂度为O ( n x ⋅ n y ⋅ n θ ) O(n_x \cdot n_y \cdot n_\theta)O(nx​⋅ny​⋅nθ​)——典型的搜索窗口7 m × 7 m × 30 ° 7m \times 7m \times 30°7m×7m×30°分辨率0.05 m × 0.05 m × 1 ° 0.05m \times 0.05m \times 1°0.05m×0.05m×1°带来( 140 × 140 × 30 ) ≈ 5.88 × 10 5 (140 \times 140 \times 30) \approx 5.88 \times 10^5(140×140×30)≈5.88×105个候选位姿.对每个候选都要计算所有扫描点的似然典型的扫描有~1080个点总计算量巨大。BB加速通过分层搜索来解决建立多分辨率搜索树根节点整个搜索窗口子节点父窗口的8个或更少等分区域叶子节点最高分辨率最终候选位姿每个节点计算匹配分数的上界在粗糙分辨率上对每个候选区域计算该区域内可能达到的最高匹配分数。剪枝Pruning如果某节点的分数上界低于当前找到的最佳匹配分数——该子树不可能包含更好的匹配被跳过。深度优先搜索从根节点深入高分辨率区域实时更新当前最佳分数。BB保证找到全局最优匹配与穷举搜索一致但将复杂度从O ( n x ⋅ n y ⋅ n θ ) O(n_x \cdot n_y \cdot n_\theta)O(nx​⋅ny​⋅nθ​)降为O ( log ⁡ ( n x ⋅ n y ⋅ n θ ) ) O(\log(n_x \cdot n_y \cdot n_\theta))O(log(nx​⋅ny​⋅nθ​))。2.3 为什么2D激光可以用穷举搜索在2D激光中回环检测本质上是SE(2)空间中的3维搜索——这是数学上易于穷举的低维空间。对比视觉SLAM的回环检测在描述子空间256维中检索——无法穷举3D激光的回环检测在SE(3)空间6维中搜索——直接穷举代价过高2D激光回环检测的独特性在于搜索空间 (3维) 恰好足够小使得全局穷举搜索可行。3. 3D激光Scan Context与几何检索3.1 Scan Context的编码原理Scan ContextKim Kim, 2018将3D点云的一帧扫描编码为一个描述性矩阵步骤1 — 极坐标分桶将3D空间沿方位角N r N_rNr​个环×N s N_sNs​个扇区分割径向等间距环如20个环每环间距Δ r \Delta rΔr方位角等角度扇区如60个扇区每扇区6 ° 6°6°步骤2 — 每桶编码每个桶环扇区的交叉区域存储该区域内最高点的高度值z zz坐标。结果是一个N r × N s N_r \times N_sNr​×Ns​矩阵如20 × 60 20 \times 6020×60每一行对应一个环每一列对应一个方位角。步骤3 — 描述子提取Scan Context矩阵的每一行一个环通过其高度分布的统计量均值、方差等编码为一个环描述向量。3.2 检索机制旋转不变性Scan Context不直接比较两个矩阵——因为两帧可能有任意的相对偏航角。检索时计算当前帧的环形描述向量在候选帧中对可能的偏航角偏移列偏移计算矩阵距离取最小距离作为该候选帧的匹配分数两阶段检索环形描述向量的KD树检索快速筛选候选帧基于环状签名排除大部分不相关帧Scan Context矩阵比较对候选帧在偏航角偏移上计算最小矩阵距离3.3 Scan Context vs 词袋模型的对比Scan Context (3D激光)词袋模型 (视觉)编码内容几何结构高度分布外观纹理描述子统计描述子维度N r × N s N_r \times N_sNr​×Ns​矩阵~1200值稀疏词袋向量~10 5 10^5105维旋转不变性列偏移最小距离无外观不依赖视角旋转对光照不变是纯几何否光照影响外观描述子对视角不变否视角变化改变几何投影部分特征描述子有一定视角容错几何退化场景空旷场地→所有环高度相似→低辨别力白墙→词汇分布无信息→低辨别力两者互补激光的几何回环检测在结构化的室内场景中非常好有丰富的垂直结构在空旷室外差视觉的外观回环检测在有丰富纹理的场景中好在弱纹理/重复纹理场景中差。LVI-SAM将两者同时使用——视觉回环检测和激光回环检测互为补充。3.4 其他3D激光回环方法Intensity Scan Context除了高度外还编码反射强度intensity在几何退化场景平坦的地面中增加了纹理维度LiDAR-Iris将Scan Context矩阵做傅里叶变换→二值化编码为虹膜码→汉明距离快速匹配4. 视觉词袋模型4.1 从连续描述子到离散词汇词袋模型将连续的特征描述子空间如256维ORB空间量化为离散的视觉词汇从大规模训练图像中提取数百万个描述子用层次化K-means聚类将描述子分为K KK个簇K b d K b^dKbdb bb为分支因子d dd为深度每个叶子节点 一个视觉词汇新图像的每个描述子分配到最近的叶子节点4.2 TF-IDF加权方案词频TF词汇w ww在图像i ii中的出现频率TF ( w , i ) n w i / n i \text{TF}(w, i) n_{wi} / n_iTF(w,i)nwi​/ni​逆文档频率IDF词汇w ww在整个图像库中稀有程度的度量IDF ( w ) log ⁡ ( N / n w ) \text{IDF}(w) \log(N / n_w)IDF(w)log(N/nw​)IDF的精髓如果某词汇在几乎所有图像中都出现如白墙的词汇它对区分不同图像没有帮助IDF→0如果某词汇仅在一帧中出现它对区分这一帧非常重要IDF大。TF-IDF词袋向量v [ word w ] TF ( w , i ) × IDF ( w ) \mathbf{v}[\text{word}_w] \text{TF}(w, i) \times \text{IDF}(w)v[wordw​]TF(w,i)×IDF(w)经L1归一化。4.3 双索引机制倒排索引Inverted Index词汇 → 包含该词汇的图像列表。用于快速筛选候选帧——仅取新图像中非零词汇对应的倒排列表的并集作为候选集在候选集上计算完整词袋相似度而非对所有历史图像。直接索引Direct Index图像 → 词汇到特征点的映射。用于加速几何验证——确认候选后仅匹配属于同一词汇的特征而非所有特征对暴力匹配。两步串联将回环检测时间从数千毫秒暴力遍历暴力匹配降至数十毫秒。4.4 视觉回环的特殊挑战Perceptual Aliasing两个外观相似但物理不同的地方perceptual aliasing会给出高词袋分数——这是视觉回环检测的固有问题。激光回环检测不受此问题影响几何结构极少偶然巧合。缓解措施几何验证用对极几何/Sim(3)剔除外观相似但几何不一致的候选时序一致性连续3帧都检测到与同一候选区域的回环才确认动态权重降低回环边的信息矩阵设为里程计边的50-70%5. 几何验证三种SLAM的共性5.1 为什么词袋/Scan Context不够无论是视觉的词袋分数还是激光的匹配分数都仅提供外观/几何层面的相似度——它们可以被欺骗视觉的perceptual aliasing激光的几何重复结构如长走廊。几何验证用刚性的多视图几何/点云配准确保回环是物理上真实的。5.2 三种SLAM的几何验证方法2D激光3D激光视觉验证方法ICP精配准ICP/NDT精配准对极几何RANSAC验证标准配准残差 阈值配准残差 阈值内点数 阈值~20-30位姿类型SE(2)刚体变换SE(3)刚体变换SE(3)双目/RGB-D或 Sim(3)单目变换估计正交ProcrustesSVD非线性优化点到线/面E矩阵三角化 或 PnP单目视觉特殊情况回环候选帧和当前帧之间不仅有旋转和平移差异还有尺度差异单目漂移过程中地图的整体尺度可能变大或变小。需要估计Sim(3)相似变换7自由度旋转3平移3均匀尺度1最少需要3对3D-3D匹配。5.3 回环确认后的处理确认回环后在位姿图中添加一条新的约束边min ⁡ T i ∑ ( i , j ) ∈ E ∥ log ⁡ ( T i j − 1 ⋅ T i − 1 ⋅ T j ) ∥ Σ i j 2 \min_{\mathbf{T}_i} \sum_{(i,j) \in \mathcal{E}} \|\log(\mathbf{T}_{ij}^{-1} \cdot \mathbf{T}_i^{-1} \cdot \mathbf{T}_j)\|_{\mathbf{\Sigma}_{ij}}^2Ti​min​(i,j)∈E∑​∥log(Tij−1​⋅Ti−1​⋅Tj​)∥Σij​2​回环边将误差修正从回环点传播到整条轨迹——远离回环的修正逐渐减弱。修正的大小取决于信息矩阵Ω i j \mathbf{\Omega}_{ij}Ωij​的相对大小信息矩阵越大回环约束越强修正传播越远。6. 位姿图优化与全局一致性6.1 位姿图优化 简化版BA位姿图仅优化位姿变量2D: 3维/帧3D: 6维/帧不涉及路标变量。对于三种SLAM位姿图优化的适用范围2D激光3D激光视觉位姿图维度3 N 3N3N6 N 6N6N6 N 6N6N典型N NN1000050001000Hessian规模30000 × 30000 30000 \times 3000030000×3000030000 × 30000 30000 \times 3000030000×300006000 × 6000 6000 \times 60006000×6000求解时间稀疏~10ms~50ms~20ms6.2 回环修正后的全局BA视觉特有视觉SLAM在位姿图优化后执行全局BA重新精化3D路标位置和相机位姿。此时初值已非常接近真值全局BA通常在几次迭代内收敛。2D/3D激光SLAM不需要这个步骤——因为激光的直接深度测量意味着路标不需要被重新估计。这是激光相对于视觉的又一个后端简化。6.3 误匹配的防御机制多帧时序一致性连续3帧检测到相同的回环区域才确认。这是所有三种SLAM通用的防御机制。信息矩阵的保守设置回环边的信息矩阵始终低于里程计边的信息矩阵——反映回环约束的更高不确定性。对勉强通过几何验证的回环如内点仅20-25进一步降低信息矩阵。错误的回环边如果信息矩阵很小对位姿图的修正可忽略不计——这是一个优雅的容错机制不是二元的接受/拒绝回环而是根据验证的置信度赋予连续的权重。本文总结三种SLAM的回环检测都遵循共同的流程特征提取 → 描述子编码 → 近似最近邻检索 → 几何验证 → 位姿图优化2D激光 分支定界BB在SE(2)空间中穷举搜索3维搜索空间刚好可穷举BB将复杂度从O ( n x n y n θ ) O(n_x n_y n_\theta)O(nx​ny​nθ​)降为O ( log ⁡ ) O(\log)O(log)—这是2D激光独有的优势3D激光 Scan Context编码 环形检索将3D点云编码为高度-方位角矩阵用列偏移处理旋转不变性与视觉词袋互补几何 vs 外观视觉 词袋模型 TF-IDF 倒排/直接双索引将图像量化为视觉词汇分布通过层次化检索和双索引机制实现实时回环几何验证是所有三种SLAM的共性步骤外观/几何相似度可被欺骗但物理几何是刚性的——对极约束视觉和ICP配准激光确保回环为真位姿图优化对三种SLAM通用回环修正从回环点传播到整条轨迹——激光SLAM不需要后续的全局BA因为深度直接测量标签SLAM回环检测分支定界Scan ContextDBoW2位姿图优化