GTSAM 库详细介绍与使用指南

一、GTSAM 概述

GTSAM（Georgia Tech Smoothing and Mapping）是由佐治亚理工学院开发的 C++ 开源库，专注于 概率图模型（尤其是因子图）的构建与优化，广泛应用于机器人定位与建图（SLAM）、传感器融合、运动规划等领域。其核心优势在于：

• 高效的因子图优化：支持贝叶斯网络建模与非线性优化。
• 增量式求解器（iSAM/iSAM2）：适用于实时 SLAM 问题。
• 多传感器融合能力：兼容 IMU、LiDAR、相机等数据。

二、核心算法原理

1. 因子图（Factor Graph）

• 数学基础：将优化问题建模为因子图，节点表示变量（如机器人的位姿、地标位置），边表示约束（如里程计、传感器观测）。
• 概率模型：联合概率分布分解为多个因子的乘积：
  [
  p(X) \propto \prod_i f_i(X_i)
  ]
  其中 (X) 为待优化变量，(f_i) 为约束因子。

2. 非线性最小二乘优化

• 目标函数：将因子图转换为最小二乘问题：
  [
  X^* = \arg\min_X \sum_i | h_i(X_i) - z_i |_{\Sigma_i}^2
  ]

  • (h_i)：观测模型
  • (z_i)：实际观测值
  • (\Sigma_i)：协方差矩阵

• 优化方法：采用 Levenberg-Marquardt（LM）或 Gauss-Newton（GN）算法求解。

3. iSAM2 增量优化

• 核心思想：通过贝叶斯树实现增量式优化，仅更新受新数据影响的局部区域。
• 优势：避免全局重新线性化，显著提升大规模 SLAM 的实时性。

三、核心功能模块

1. 因子类型

• BetweenFactor：描述两个变量间的相对约束（如里程计）。
• PriorFactor：为变量添加先验约束。
• GenericProjectionFactor：处理相机投影模型（如视觉 SLAM）。
• ImuFactor：融合 IMU 预积分数据。

2. 变量类型

• Pose2：2D 位姿（x, y, theta）。
• Pose3：3D 位姿（x, y, z, roll, pitch, yaw）。
• Point2/Point3：2D/3D 点坐标。

3. 工具类

• NonlinearFactorGraph：管理因子图结构。
• Values：存储变量初始值与优化结果。
• ISAM2：增量式求解器。

四、应用场景

1. 机器人 SLAM
   • 激光雷达建图（LiDAR SLAM）。
   • 视觉惯性里程计（VIO）。
2. 自动驾驶
   • 多传感器融合（LiDAR + 相机 + IMU）。
   • 高精度地图优化。
3. 无人机导航
   • 实时路径规划与避障。
4. AR/VR
   • 环境跟踪与虚拟对象定位。

五、GTSAM 安装与配置

1. 依赖项

• CMake（≥3.0）
• Boost（≥1.65）
• Eigen（≥3.3）

2. 安装步骤

git clone https://github.com/borglab/gtsam.git
cd gtsam && mkdir build && cd build
cmake -DGTSAM_BUILD_EXAMPLES=ON ..
make -j4
sudo make install

六、使用示例

示例 1：2D 位姿优化（位姿图优化）

#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Marginals.h>

using namespace gtsam;

int main() {
    // 1. 初始化因子图和变量
    NonlinearFactorGraph graph;
    Values initial;

    // 2. 添加先验因子（固定初始位姿）
    Pose2 prior_pose(0, 0, 0);
    noiseModel::Diagonal::shared_ptr prior_noise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
    graph.addPrior(1, prior_pose, prior_noise);

    // 3. 添加里程计约束（BetweenFactor）
    Pose2 odom(2, 0, 0); // 从位姿1到位姿2的位移
    noiseModel::Diagonal::shared_ptr odom_noise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
    graph.emplace_shared<BetweenFactor<Pose2>>(1, 2, odom, odom_noise);

    // 4. 设置初始值
    initial.insert(1, Pose2(0, 0, 0));
    initial.insert(2, Pose2(1.9, 0.1, 0.05)); // 添加噪声的初始猜测

    // 5. 优化
    LevenbergMarquardtParams params;
    LevenbergMarquardtOptimizer optimizer(graph, initial, params);
    Values result = optimizer.optimize();

    // 6. 输出结果
    result.print("Final Result:");
    return 0;
}

示例 2：视觉惯性里程计（VIO）

// 包含 IMU 因子和视觉投影因子
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/slam/SmartProjectionPoseFactor.h>

// 定义 IMU 预积分参数
PreintegratedImuMeasurements imu_params = PreintegratedImuMeasurements::Params::MakeSharedU(9.81);
imu_params->gyroscopeCovariance = Matrix3::Identity(3,3) * 1e-4;
imu_params->accelerometerCovariance = Matrix3::Identity(3,3) * 1e-2;

// 创建 IMU 因子
ImuFactor imu_factor(key_pose1, key_vel1, key_pose2, key_vel2, key_imu_bias, imu_measurements);

// 创建视觉投影因子
noiseModel::Isotropic::shared_ptr measurement_noise = noiseModel::Isotropic::Sigma(2, 1.0);
SmartProjectionPoseFactor::shared_ptr vision_factor(new SmartProjectionPoseFactor(measurement_noise));
vision_factor->add(measurement, key_pose, key_landmark, K); // K为相机内参
graph.add(vision_factor);

七、高级功能

1. 增量式优化（iSAM2）

#include <gtsam/nonlinear/ISAM2.h>

ISAM2Params params;
params.relinearizeThreshold = 0.01; // 重新线性化阈值
ISAM2 isam(params);

// 逐步添加因子并更新
for (int i = 0; i < steps; ++i) {
    NonlinearFactorGraph new_factors;
    Values new_values;
    // 添加新因子和变量
    isam.update(new_factors, new_values);
    Values result = isam.calculateEstimate();
}

2. 协方差估计

Marginals marginals(graph, result);
Matrix covariance = marginals.marginalCovariance(key); // 获取某个变量的协方差矩阵

八、性能优化技巧

1. 稀疏性利用：通过设置合理的变量顺序（Variable Ordering）加速求解。
2. 噪声模型选择：根据传感器特性选择合适的噪声模型（如 Diagonal 或 Robust）。
3. 内存管理：定期调用 isam.relinearizeThreshold 控制增量更新的频率。

九、与其他库的集成

• ROS：通过 gtsam_ros 包发布优化后的位姿和地图。
• OpenCV：结合视觉特征提取与匹配。
• PCL：处理 LiDAR 点云数据。

十、常见问题与调试

1. 优化发散
   • 检查初始值是否合理。
   • 降低 LM 算法的初始阻尼系数（params.dampingFactor）。
2. 内存泄漏
   • 使用 Valgrind 工具检测 C++ 代码。
3. 编译错误
   • 确保所有依赖项版本兼容。

十一、学习资源

1. 官方文档：https://gtsam.org/
2. GitHub 示例：https://github.com/borglab/gtsam/tree/develop/examples
3. 书籍推荐：《Factor Graphs for Robot Perception》

通过上述内容，您可以快速掌握 GTSAM 的核心功能，并应用于实际项目中。其强大的因子图优化能力与高效的增量求解器使其成为机器人领域的重要工具。