GTSAM 库介绍与使用指南

GTSAM 库介绍与使用指南

1. GTSAM 简介

GTSAM (Georgia Tech Smoothing and Mapping) 是一个开源的C++库，用于解决机器人技术中的传感器融合问题，特别是同时定位与地图构建(SLAM)和运动结构恢复(SfM)问题。它实现了因子图模型和贝叶斯网络，提供了高效的推理算法。

主要特点：

• 基于因子图的概率推理框架
• 支持多种传感器模型和约束类型
• 提供非线性优化求解器
• 支持增量式推理(ISAM2)
• 包含2D和3D几何工具

2. 安装 GTSAM

Ubuntu 安装

sudo add-apt-repository ppa:borglab/gtsam-release-4.1
sudo apt update
sudo apt install libgtsam-dev libgtsam-unstable-dev

从源码编译

git clone https://github.com/borglab/gtsam.git
cd gtsam
mkdir build && cd build
cmake ..
make -j8
sudo make install

3. 基本使用示例

示例1：简单的2D位姿图优化

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

using namespace gtsam;

int main() {
    // 1. 创建因子图
    NonlinearFactorGraph graph;
    
    // 2. 添加先验因子 (在pose1处的先验)
    Pose2 priorMean(0.0, 0.0, 0.0); // x, y, theta
    noiseModel::Diagonal::shared_ptr priorNoise = 
        noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
    graph.addPrior(1, priorMean, priorNoise);
    
    // 3. 添加里程计因子 (从pose1到pose2)
    Pose2 odometry(2.0, 0.0, 0.0);
    noiseModel::Diagonal::shared_ptr odometryNoise = 
        noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
    graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
    
    // 4. 添加闭环因子 (从pose2回到pose1)
    Pose2 loopClosure(-2.0, 0.0, 0.0);
    noiseModel::Diagonal::shared_ptr loopNoise = 
        noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
    graph.add(BetweenFactor<Pose2>(2, 1, loopClosure, loopNoise));
    
    // 5. 创建初始估计
    Values initial;
    initial.insert(1, Pose2(0.5, 0.0, 0.2)); // 初始估计有误差
    initial.insert(2, Pose2(2.3, 0.1, -0.2));
    
    // 6. 优化
    LevenbergMarquardtOptimizer optimizer(graph, initial);
    Values result = optimizer.optimize();
    
    // 7. 打印结果
    result.print("Final Result:");
    
    return 0;
}

示例2：3D点云配准

#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Values.h>

using namespace gtsam;

int main() {
    // 1. 创建因子图
    NonlinearFactorGraph graph;
    
    // 2. 添加相机位姿先验
    Pose3 priorPose(Rot3::RzRyRx(0.1, -0.1, 0.2), Point3(1.0, 0.5, 1.0));
    noiseModel::Diagonal::shared_ptr poseNoise = 
        noiseModel::Diagonal::Sigmas((Vector(6) << 0.3, 0.3, 0.3, 0.1, 0.1, 0.1).finished());
    graph.addPrior(1, priorPose, poseNoise);
    
    // 3. 添加3D点先验
    Point3 landmark(5.0, 3.0, 2.0);
    noiseModel::Diagonal::shared_ptr pointNoise = 
        noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));
    graph.addPrior(100, landmark, pointNoise); // 使用大数字表示地标
    
    // 4. 添加投影因子 (观测)
    Cal3_S2::shared_ptr K(new Cal3_S2(500.0, 500.0, 0.0, 640.0/2, 480.0/2));
    Point2 measurement(323.0, 240.0);
    noiseModel::Isotropic::shared_ptr measurementNoise = 
        noiseModel::Isotropic::Sigma(2, 2.0); // 像素误差
    graph.add(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(
        measurement, measurementNoise, 1, 100, K));
    
    // 5. 创建初始估计
    Values initial;
    initial.insert(1, Pose3(Rot3::RzRyRx(0.0, 0.0, 0.0), Point3(1.1, 0.4, 0.9)));
    initial.insert(100, Point3(5.1, 2.9, 2.1));
    
    // 6. 优化
    LevenbergMarquardtOptimizer optimizer(graph, initial);
    Values result = optimizer.optimize();
    
    // 7. 打印结果
    result.print("Final Result:");
    
    return 0;
}

示例3：使用ISAM2进行增量式SLAM

#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Pose2.h>

using namespace gtsam;

int main() {
    // 1. 创建ISAM2对象
    ISAM2Params parameters;
    parameters.relinearizeThreshold = 0.01;
    parameters.relinearizeSkip = 1;
    ISAM2 isam(parameters);
    
    // 2. 创建因子图和初始值
    NonlinearFactorGraph graph;
    Values initialEstimate;
    
    // 3. 添加先验
    Pose2 priorMean(0.0, 0.0, 0.0);
    noiseModel::Diagonal::shared_ptr priorNoise = 
        noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
    graph.addPrior(1, priorMean, priorNoise);
    initialEstimate.insert(1, Pose2(0.0, 0.0, 0.0));
    
    // 4. 第一次更新
    isam.update(graph, initialEstimate);
    isam.update();
    
    // 5. 清空临时变量
    graph.resize(0);
    initialEstimate.clear();
    
    // 6. 添加第一个里程计测量
    Pose2 odometry1(1.0, 0.0, 0.0);
    noiseModel::Diagonal::shared_ptr odometryNoise = 
        noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
    graph.add(BetweenFactor<Pose2>(1, 2, odometry1, odometryNoise));
    initialEstimate.insert(2, Pose2(1.0, 0.0, 0.0));
    
    // 7. 第二次更新
    isam.update(graph, initialEstimate);
    isam.update();
    
    // 8. 清空临时变量
    graph.resize(0);
    initialEstimate.clear();
    
    // 9. 添加第二个里程计测量
    Pose2 odometry2(1.0, 0.0, M_PI_2);
    graph.add(BetweenFactor<Pose2>(2, 3, odometry2, odometryNoise));
    initialEstimate.insert(3, Pose2(1.0, 1.0, M_PI_2));
    
    // 10. 第三次更新
    isam.update(graph, initialEstimate);
    isam.update();
    
    // 11. 获取当前估计
    Values currentEstimate = isam.calculateEstimate();
    currentEstimate.print("Current Estimate:");
    
    return 0;
}

4. GTSAM 主要组件

1. 因子图 (Factor Graph): 表示问题的概率模型

   • NonlinearFactorGraph: 非线性因子图
   • GaussianFactorGraph: 高斯因子图

2. 优化器 (Optimizers):

   • LevenbergMarquardtOptimizer: Levenberg-Marquardt 非线性优化
   • GaussNewtonOptimizer: 高斯-牛顿优化
   • ISAM2: 增量平滑和建图算法

3. 几何类型:

   • Pose2: 2D位姿 (x,y,θ)
   • Pose3: 3D位姿 (旋转矩阵+平移向量)
   • Point2, Point3: 2D/3D点
   • Rot2, Rot3: 2D/3D旋转

4. 因子 (Factors):

   • BetweenFactor: 位姿间约束
   • PriorFactor: 先验约束
   • ProjectionFactor: 投影因子

5. 噪声模型:

   • noiseModel::Gaussian
   • noiseModel::Diagonal
   • noiseModel::Isotropic

5. 实际应用建议

1. 选择合适的优化器:

   • 对于小规模问题: 使用LevenbergMarquardtOptimizer
   • 对于大规模SLAM问题: 使用ISAM2

2. 噪声模型选择:

   • 传感器测量精度越高，噪声协方差应设置越小
   • 闭环检测通常比里程计有更大的不确定性

3. 调试技巧:

   • 从简单问题开始，逐步增加复杂性
   • 检查优化前后的残差变化
   • 使用Marginals类计算协方差矩阵

4. 性能优化:

   • 使用Symbol代替整数键可以提高效率
   • 对于固定变量，使用FixedLagSmoother

GTSAM是一个功能强大但有一定学习曲线的库，建议从简单的2D例子开始，逐步扩展到更复杂的3D问题。