GTSAM中增量式添加地标点的技术解析

概述

在视觉SLAM系统中，地标点（Landmarks）的管理是一个核心问题。本文基于borglab/gtsam项目中的一个讨论，深入探讨如何在iSAM2框架下实现地标点的增量式添加，而非传统的一次性初始化方式。

传统地标点添加方式的问题

在GTSAM的示例代码VisualISAM2Example.cpp中，我们可以看到传统的地标点处理方式：

1. 在第一个位姿处添加所有地标点的初始估计
2. 为第一个地标点添加先验约束
3. 随后在循环中逐步添加观测约束

这种方式虽然简单，但与实际情况存在明显差异：

• 单目视觉系统中，地标点的3D位置无法在第一帧就确定
• 实际应用中，地标点是随着相机运动逐步被发现和初始化的
• 部分地标点可能在后续帧中消失

增量式地标点管理的实现

要实现更符合实际情况的增量式地标点管理，我们需要对传统方法进行以下改进：

1. 延迟地标点初始化

不应在第一帧就初始化所有地标点，而应该：

• 等待足够多的观测以进行三角测量
• 仅当3D位置可估计时才添加到图中
• 为第一个成功初始化的地标点添加先验约束

2. 渐进式地标点添加

在每一帧处理时：

for (size_t i = 0; i < poses.size(); ++i) {
    // 仅添加当前帧观测到的地标点
    for (size_t j : observedLandmarksInCurrentFrame) {
        if (!initialEstimate.exists(Symbol('l', j))) {
            // 新发现的地标点，进行初始化
            initialEstimate.insert<Point3>(Symbol('l', j), estimatedPosition);
        }
        // 添加观测约束
        graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2>>(...);
    }
    // 更新iSAM2
    isam.update(graph, initialEstimate);
}

3. 单目SLAM的特殊考虑

对于单目系统，还需要特别注意：

• 需要等待足够视差才能初始化地标点
• 可考虑使用逆深度参数化（Inverse Depth Parameterization）
• 实现地标点的延迟初始化策略

逆深度参数化的扩展

GTSAM的gtsam_unstable模块提供了三种逆深度因子：

1. InverseDepthFactor3
2. InverseDepthFactorVariant1
3. InverseDepthFactorVariant2

这些因子更适合单目SLAM中的地标点表示，可以：

• 更好地处理远距离地标点
• 提供更稳定的初始化
• 改善优化收敛性

实际应用建议

在实际应用中，建议开发者：

1. 根据传感器类型选择合适的初始化策略
2. 实现地标点的生命周期管理
3. 考虑使用滑动窗口管理地标点
4. 针对单目系统实现专门的三角测量和初始化逻辑

总结

GTSAM提供了强大的增量式优化能力，但要充分发挥其潜力，需要根据实际应用场景合理设计地标点管理策略。通过增量式添加地标点、延迟初始化和适当的参数化方法，可以构建更鲁棒、更符合实际情况的SLAM系统。