GTSAM(Georgia Tech Smoothing and Mapping)是由佐治亚理工学院开发的 C++ 开源库,专注于 概率图模型(尤其是因子图)的构建与优化,广泛应用于机器人定位与建图(SLAM)、传感器融合、运动规划等领域。其核心优势在于:
目标函数:将因子图转换为最小二乘问题:
[
X^* = \arg\min_X \sum_i | h_i(X_i) - z_i |_{\Sigma_i}^2
]
优化方法:采用 Levenberg-Marquardt(LM)或 Gauss-Newton(GN)算法求解。
git clone https://github.com/borglab/gtsam.git
cd gtsam && mkdir build && cd build
cmake -DGTSAM_BUILD_EXAMPLES=ON ..
make -j4
sudo make install
#include
#include
#include
#include
#include
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;
}
// 包含 IMU 因子和视觉投影因子
#include
#include
// 定义 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);
#include
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();
}
Marginals marginals(graph, result);
Matrix covariance = marginals.marginalCovariance(key); // 获取某个变量的协方差矩阵
Diagonal
或 Robust
)。isam.relinearizeThreshold
控制增量更新的频率。gtsam_ros
包发布优化后的位姿和地图。params.dampingFactor
)。Valgrind
工具检测 C++ 代码。通过上述内容,您可以快速掌握 GTSAM 的核心功能,并应用于实际项目中。其强大的因子图优化能力与高效的增量求解器使其成为机器人领域的重要工具。