MULLS 后端全解析：回环检测、TEASER++ 全局配准与位姿图优化（Ceres/g2o/GTSAM）深度解读
MULLS 后端全解析回环检测、TEASER 全局配准与位姿图优化Ceres/g2o/GTSAM深度解读【免费下载链接】MULLS MULLS: Versatile LiDAR SLAM via Multi-metric Linear Least Square [ICRA 21]项目地址: https://gitcode.com/gh_mirrors/mu/MULLSMULLSMulti-metric Linear Least Square多度量线性最小二乘是 ICRA 2021 上提出的一款多功能 LiDAR SLAM 系统它的**后端Pose Graph Optimization**由回环检测、TEASER 全局配准和位姿图优化三大部分组成并同时支持 Ceres、g2o、GTSAM 三种主流优化库。本文将从源码层面为你深度解读 MULLS 后端的完整工作流程帮助新手理解 LiDAR SLAM 中前端建图、后端闭环的经典套路无需阅读大量代码也能看懂核心设计。MULLS 后端整体架构从里程计到位姿图的闭环链路先看全局。MULLS 的在线建图采用子图Submap / Strip分层管理车辆每前进一段距离累计平移约 12 米、累计旋转约 60°、或累计 100 帧就切出一个新子图子图与子图之间用相邻约束ADJACENT串联形成一条条带Strip链。当轨迹走成闭环时系统通过回环检测找到空间上相近但时间上不相邻的子图对再用全局配准算出它们的相对位姿最终交给位姿图优化把整条轨迹拉回一致。这一整套逻辑分布在项目的 include/pgo/build_pose_graph.h、include/pgo/graph_optimizer.h 和 include/pgo/map_manager.h 三个核心头文件中。后端流水线可概括为四步子图切分 → 回环候选搜索 → 全局配准验证 → 位姿图优化对应源码中Constraint_Finder、CRegistration、GlobalOptimize三个类。下面逐层拆解。回环检测怎么做KD-Tree 近邻搜索与 IoU 判定的候选边挖掘回环检测的目的是找出曾经来过的地方。MULLS 采用基于几何位置的回环检测而不是视觉词袋或学习式描述子实现非常轻量KD-Tree 近邻搜索把已有子图的中心点2D 的 x,y 或 3D 的 x,y,z建成 KD-Tree每来一个新子图就在给定半径内搜索邻居默认最多取 10 个候选。见Constraint_Finder::find_overlap_registration_constraintsrc/build_pose_graph.cpp。IoU 重叠度过滤对每个候选对计算二维包围盒 IoUcalculate_iou只有重叠度足够高默认阈值 0.25的才算真正回到旧地点。时间邻域剔除通过judge_adjacent_by_id剔除 ID 差小于 3 的相邻子图避免把普通的相邻约束误判成回环边。通过三重过滤的候选对被标记为REGISTRATION回环约束并按其 IoU 从大到小排序重叠度高的优先配准。配准前还会调用double_check_tran做一次交叉验证如果全局配准结果与里程计预测的位姿差异过大默认平移 8 米、旋转 45°系统会信任里程计而不是全局配准从而防止错误回环把整个地图带偏。TEASER 全局配准为什么比 RANSAC 更快更稳回环边拿到手后需要一个不受初始位姿影响、能容忍大量外点的全局配准器来算相对位姿。MULLS 默认使用 MIT 开源的TEASER并在 include/common/cregistration.hpp 中封装为coarse_reg_teaser截断最小二乘TLSTEASER 用 GNC-TLS 算法估计旋转配合最大团Max-Clique求解平移对外点比例高达 90% 以上的情形依然稳健这是它相比 RANSAC 的核心优势。关键参数可调noise_bound噪声界默认 0.2、min_inlier_num最小内点数、cbar2、rotation_gnc_factor等都通过 GFlags 配置脚本里还针对不同线数激光雷达设置了不同的teaser_min_inlier_count如 16 线默认 12、32/64 线默认 7~8。三级状态返回配准结束后返回-1失败、0成功但需人工复查、1内点数超过 2 倍阈值判定可靠三种状态供上层决定是否把这条回环边加入优化。在 CMake 中只要BUILD_WITH_TEASER开启默认 ON就会链接teaserpp若关闭则自动回退到 RANSAC 方案。运行script/run_mulls_reg.sh即可体验基于 TEASER 的点云对配准 Demo效果见下图演示。位姿图优化三大后端深度对比Ceres、g2o、GTSAM回环边和相邻边汇聚成一张位姿图接下来就是重头戏——位姿图优化PGO。MULLS 的GlobalOptimize类src/graph_optimizer.cpp同时实现了三套后端通过宏开关CERES_ON、G2O_ON、GTSAM_ON切换官方推荐优先使用 Ceres。Ceres 优化流程推荐首选自动求导 稀疏求解Ceres 路径的核心是自定义的四元数残差PoseGraph3dErrorTermQUATinclude/pgo/graph_optimizer.h每个节点用(x,y,z,qx,qy,qz,qw)7 维参数表示残差为 6 维3 维平移误差 3 维旋转误差通过AutoDiffCostFunction自动求导。求解器配置上小规模问题用DENSE_SCHUR大规模稀疏问题自动切换SPARSE_SCHUR依赖 SuiteSparse并支持多线程与HuberLoss鲁棒核函数。Ceres 路径还有一个独特的**分层限位策略**set_pgo_problem_ceres会把节点分为三类——最早期回环之前的节点固定Fixed、已稳定的节点给定小范围边界Stable、较新的节点按距稳定点的距离成比例放宽边界Moving通过SetParameterLowerBound/UpperBound实现这能有效防止优化把前端已收敛的局部结构破坏。g2o 优化流程经典图优化框架LM 与 Dogleg 双策略g2o 路径使用EdgeSE3Expmap边和VertexSE3Expmap顶点src/graph_optimizer.cpp支持Levenberg-Marquardt与Dogleg两种信赖域策略线性求解器可选 Dense、Cholmod稀疏或 PCG。与 Ceres 一致地支持 Huber 鲁棒核和等权/信息矩阵两种加权模式。代码注释中特别提醒了一个 g2o 的坑g2o 的边方向是从 vertex1 指向 vertex0与约束中Trans1_2block2→block1方向相反必须正确映射否则优化结果会出错。GTSAM/ISAM2 优化流程面向增量式 SLAM 的因子图方案GTSAM 路径include/pgo/graph_optimizer.h采用因子图建模NonlinearFactorGraph装载BetweenFactor边与PriorFactor先验噪声模型分为里程计噪声、回环约束噪声和鲁棒噪声三层并内置了ISAM2 增量优化器——适合长时间在线运行时边来边优化无需每次都全量重算。这也是作者在源码注释中建议最好用 GTSAM类似 LeGO-LOAM 的做法的原因。鲁棒性设计信息矩阵、Huber 核与错误边剔除后端精度不仅靠求解器更靠一整套鲁棒性机制信息矩阵加权每条边携带 6×6 信息矩阵协方差逆可选等权模式、仅取对角元素、或完整的矩阵平方根分解information_matrix.sqrt()三种方式回环边的协方差还会在每次优化后用update_edge_covariance_ceres更新实现边越可信权重越高的自适应。鲁棒核函数Ceres 用HuberLoss、g2o 用RobustKernelHuber把回环误匹配这类大残差的影响降级为线性惩罚避免单条坏边毁掉全局。错误边二次校验update_optimized_edges会基于优化结果反查每条边若某条边的位姿变化超过平移/旋转阈值如inter_submap_t_limit0.8、inter_submap_r_limit0.01判定为错误边并剔除低置信度confidence 0.2或高方差的 REGISTRATION 边也会被降级为 HISTORY 边。若错误边过多导致校验失败优化会被判定为失败并回退。如何配置与运行 MULLS 后端想亲手跑通这套后端只需三步拉取代码git clone https://gitcode.com/gh_mirrors/mu/MULLS。安装依赖按项目脚本script/tools/install_dep_lib.sh安装 PCL、G2O、Ceres、GTSAM、TEASER 等库Ceres 为推荐必装项。修改配置并运行在script/config/下按传感器型号选择 GFlags 配置如 lo_gflag_list_64.txt、lo_gflag_list_kitti_urban.txt开启--loop_closure_detection_ontrue与--teaser_based_global_registration_ontrue然后执行script/run_mulls_slam.sh体验完整 SLAM或用script/run_mulls_reg.sh单独测试 TEASER 全局配准。总结MULLS 的后端是一套教科书级的 LiDAR SLAM 闭环方案基于 KD-Tree 与 IoU 的轻量回环检测大幅降低了计算开销TEASER 全局配准用截断最小二乘保证了高外点场景下的鲁棒性而Ceres/g2o/GTSAM 三后端 信息矩阵 鲁棒核 错误边剔除的组合则让整张位姿图始终稳在正确的解上。对于想深入 LiDAR SLAM 的开发者MULLS 的 src/build_pose_graph.cpp、src/graph_optimizer.cpp 与 include/common/cregistration.hpp 三份源码就是最值得精读的实战教材。【免费下载链接】MULLS MULLS: Versatile LiDAR SLAM via Multi-metric Linear Least Square [ICRA 21]项目地址: https://gitcode.com/gh_mirrors/mu/MULLS创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考
)
