• 机器人
• 自动驾驶

【免费下载链接】Voxel-SLAM

项目地址： https://gitcode.com/gh_mirrors/vo/Voxel-SLAM

点击查看 免费下载

一、为什么需要闭环优化？

在长时间 LiDAR 导航中，激光惯性 SLAM（LiDAR-Inertial SLAM） 的累积漂移是绕不开的问题。Voxel-SLAM 给出的答案是完整的闭环链路：BTC 场景识别召回候选帧 → Plane ICP 几何验证 → 漂移检查后加入闭环边 → 基于 GTSAM ISAM2 的位姿图优化（PGO）全局修正。

整条链路在系统总览图中对应最底部的 LOOP CLOSURE 模块：

数据流非常清晰：Keyframe accumulation → Loop descriptor extraction → Loop detection in multi sessions → Multi-session PGO，并支持跨会话（Multi-session）闭环。下面逐层拆解源码实现。

二、第一步：BTC 场景识别，召回闭环候选帧

2.1 什么是 BTC？

BTC（Binary Tri-Descriptor，三元二进制描述子） 是 Voxel-SLAM 自研的场景识别描述子，定义在 BTC.h 中。其核心思路：

• 把点云按 1m 体素（voxel_size = 1.0） 切分，在体素内做平面检测（协方差最小特征值判据）；
• 从相邻投影平面的交界处提取角点，编码为 48 位的二进制向量；
• 每 3 个共面角点组成一个三角 STD（STD 结构体），携带 3 个二进制描述子 binary_A/B/C 和空间坐标，存入哈希数据库 data_base_。

💡 二进制编码的优势：相似度计算只需按位异或，一次扫描即可在海量历史关键帧中快速检索。

2.2 两级检索：粗召回 + 投票精校验

主流程中每保存一个关键帧，就调用一次检索入口：

// VoxelSLAM/src/voxelslam.cpp 第 1989 行
std_managers[id]->SearchLoop(stds_vec, search_result,
    loop_transform, loop_std_pair, std_manager->plane_cloud_vec_.back());

SearchLoop 的实现位于 BTC.cpp，分两步：

1. 候选召回（candidate_selector）：对当前每个 STD 描述子，在其周围 ±1 体素的哈希桶里查找历史描述子，按二进制相似度（rough_dis_threshold_ = 0.03）打分，选出最多 candidate_num_ = 50 个候选帧，且必须与当前帧间隔超过 skip_near_num_ = 20 帧，避免"原地打转"误判；
2. 几何精校验（candidate_verify）：对每对匹配的 STD，用 SVD 闭式解（triangle_solver）算出候选相对位姿 (rot, t)，再让所有匹配点对该位姿投票，取票数最高的位姿；最终用平面点到平面误差给出 verify_score。

只有 verify_score > icp_threshold_ (0.7) 时，该候选帧才会被视为有效闭环。

三、第二步：Plane ICP 验证，杜绝错误闭环

通过描述子检索的闭环仍可能受重复纹理（例如长走廊）干扰，因此 Voxel-SLAM 用一次法向约束的点到平面 ICP 做最终几何验证，核心函数 icp_normal 位于 loop_refine.hpp：

• 匹配条件三重过滤：法向差 < 0.2、点到平面距离 < 0.5、点间距离 < 3m，三者同时满足才计入残差；
• 最小二乘求解：每轮迭代累积 Hessian 与雅可比转置，通过 Hess.ldlt().solve(-JacT) 得到 6 自由度增量，更新 (R, t)；
• 两阶段收敛：初始宽容参数收敛后自动收紧到 0.1 / 0.1 / 0.1 / 1，二次收敛才判定成功；
• 最终判据：统计所有参与匹配的法向外积矩阵 mat_norm，其特征值最小值必须大于 icp_eigval（avia 配置中为 10），说明匹配法向分布充分、非退化，同时要求 ICP 真正收敛（is_converge == 1）。

在 voxelslam.cpp 中，验证对象是当前关键帧与候选帧各自的平面点云（plane_cloud_vec_），而非完整点云——平面特征对退化最鲁棒：

if(icp_normal(*(std_manager->plane_cloud_vec_.back()),
             *(std_managers[id]->plane_cloud_vec_[search_result.first]),
             loop_transform, icp_eigval))

四、第三步：漂移检查与闭环边入图

即使 Plane ICP 通过，Voxel-SLAM 还会计算漂移率（drift / 距离）做最后一道保险：

• 同一会话内闭环：drift_p / span < ratio_drift (0.01) 才接受，且漂移量需超过 0.10m 才触发优化；
• 跨会话闭环：drift_p / jours[id] < 0.05 才接受；
• 接受闭环后调用 lp_edges.push(...) 把闭环边存入 PGO_Edges（loop_refine.hpp），同一对会话间的多次闭环会累积在同一条边上（多次 ICP 估计 + 各自的 6D 协方差），供后续 GTSAM 做信息融合。

五、第四步：GTSAM ISAM2 位姿图优化（PGO）

满足 relc_counts > curr_halt (10) 次关键帧且存在有效闭环时，触发 isOpt = true，进入优化阶段（voxelslam.cpp）：

5.1 建图：build_graph

build_graph 函数完成三件事：

1. 通过 lp_edges.connect(cur_id, ids) 广度优先搜索，找出与当前闭环点连通的所有会话，优化范围被限制在连通子图内，保证效率；
2. 每个关键帧位姿作为一个 gtsam::Pose3 变量插入 initial，相邻帧间用带协方差的 BetweenFactor（局部 BA 输出的 v6 方差）连接成链；
3. 用 PriorFactor 以 1e-9 的极小方差锚定第一会话的首帧，消除位姿图的自由度漂移；闭环边则以 ICP 估计的位姿 + 累积协方差加入。

5.2 增量求解：ISAM2

gtsam::ISAM2Params parameters;
parameters.relinearizeThreshold = 0.01;
parameters.relinearizeSkip = 1;
gtsam::ISAM2 isam(parameters);
isam.update(graph, initial);
for(int i = 0; i < 5; i++) isam.update();

ISAM2（Incremental Sparse Adjustment）是 GTSAM 的增量式求解器：利用稀疏 Cholesky 分解的增量更新，只需几毫秒即可解出上万变量的位姿图，非常适合在线闭环触发即时的全局修正。优化完成后，新位姿通过 set_state(...) 回写到所有关键帧，重定位地图（kd-tree）也随之更新。

六、关键参数速查表

闭环相关参数集中在 config/avia.yaml 的 Loop 段，调优时重点关注：

参数	默认值	作用
jud_default	0.5	闭环评分门限（当前会话）
icp_eigval	10	Plane ICP 法向覆盖度判据
ratio_drift	0.01	同会话漂移率门限
curr_halt / prev_halt	10 / 10	触发 PGO 的关键帧间隔
Loop/jud_default 对应多会话	0.50/0.45/...	在 previous_map 配置中按会话指定

⚠️ 提示：ratio_drift 越小越保守，闭环越少但越可靠；icp_eigval 越大，对重复纹理场景的误闭环过滤越强。

七、小结

Voxel-SLAM 的闭环优化链路可以概括为一条由粗到精、层层过滤的流水线：

1. BTC 场景识别：体素化 + 平面/角点提取 + 二元编码哈希检索，毫秒级召回候选帧；
2. 三角投票 + SVD 位姿估计：从描述子匹配直接解出相对位姿；
3. Plane ICP 验证：法向约束点到平面 ICP + 特征值覆盖度判据，剔除重复纹理误配；
4. 漂移率检查：闭环位移与距离的比值门限，防"伪闭环"；
5. GTSAM ISAM2 PGO：增量位姿图优化在线修正全局轨迹，支持多会话累积建图。

这套机制让 Voxel-SLAM 在 HILTI 2023 与 ICCV 2023 SLAM 挑战赛（LiDAR 惯性赛道）中拿到了优异成绩，也使其成为研究激光惯性 SLAM 闭环检测与多会话建图的实用开源范本。

• 机器人
• 自动驾驶

【免费下载链接】Voxel-SLAM

项目地址： https://gitcode.com/gh_mirrors/vo/Voxel-SLAM

点击查看 免费下载