从理论到实践：Ceres、G2O与GTSAM在位姿图优化中的核心实现与对比

1. 位姿图优化从理论到代码的完整视角想象你正在搭建一个室内扫地机器人它需要同时完成两件事构建房间地图Mapping和确定自身位置Localization。这就是典型的SLAM问题。而位姿图优化Pose Graph Optimization正是解决这类问题的利器——它把机器人的运动轨迹抽象成位姿节点把传感器观测数据转化为位姿边通过优化这些节点和边的约束关系得到全局一致的地图。为什么需要专门研究优化库我在实际项目中遇到过这样的场景当机器人连续运动200米后单纯依靠激光里程计会产生15cm的累积误差。而采用位姿图优化后误差可以控制在3cm以内。目前主流的三大优化库各有特点CeresGoogle出品适合快速验证算法原型G2O专为图优化设计在SLAM领域应用广泛GTSAM基于因子图理论支持增量式优化下面这张表格直观展示了三者的核心差异特性CeresG2OGTSAM优化模型最小二乘图优化因子图求导方式自动/数值/解析自动/解析自动/解析流形优化需自定义内置支持内置支持稀疏求解支持专门优化专门优化学习曲线平缓陡峭中等2. Ceres实战从零构建位姿图优化2.1 Ceres的核心运作机制Ceres的优化流程就像组装乐高积木。首先需要构建代价函数CostFunction这相当于积木的基础模块。我在处理2D激光SLAM问题时通常会这样定义残差struct PoseResidual { PoseResidual(double x, double y, double yaw) : observed_x(x), observed_y(y), observed_yaw(yaw) {} template typename T bool operator()(const T* const pose1, const T* const pose2, T* residual) const { // 计算预测的相对位姿 T pred_x pose2[0] - pose1[0]; T pred_y pose2[1] - pose1[1]; T pred_yaw pose2[2] - pose1[2]; // 与观测值比较得到残差 residual[0] pred_x - T(observed_x); residual[1] pred_y - T(observed_y); residual[2] ceres::normalize_angle(pred_yaw - T(observed_yaw)); return true; } private: double observed_x, observed_y, observed_yaw; };这里有几个关键点需要注意使用模板函数实现自动求导角度残差需要特殊处理normalize_angle残差维度要与观测值维度一致2.2 信息矩阵的处理技巧与G2O不同Ceres不直接支持信息矩阵。我们需要通过Cholesky分解将马氏距离转换为最小二乘形式。具体实现如下Eigen::Matrix3d information ... // 获取信息矩阵 Eigen::Matrix3d sqrt_info information.llt().matrixL(); // 在残差计算最后添加 residual[0] sqrt_info(0,0)*residual[0] sqrt_info(0,1)*residual[1] sqrt_info(0,2)*residual[2]; residual[1] sqrt_info(1,0)*residual[0] sqrt_info(1,1)*residual[1] sqrt_info(1,2)*residual[2]; residual[2] sqrt_info(2,0)*residual[0] sqrt_info(2,1)*residual[1] sqrt_info(2,2)*residual[2];2.3 流形优化的实现当处理3D位姿SE3时需要特别注意流形结构。Ceres要求我们自定义LocalParameterizationclass SE3Parameterization : public ceres::LocalParameterization { public: virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const { Eigen::Mapconst Eigen::Vector3d trans(x); Eigen::Mapconst Eigen::Quaterniond quat(x3); Eigen::Vector3d delta_trans(delta); Eigen::Quaterniond delta_quat Eigen::Quaterniond( Eigen::AngleAxisd(delta[3], Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(delta[4], Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(delta[5], Eigen::Vector3d::UnitX())); Eigen::MapEigen::Vector3d new_trans(x_plus_delta); Eigen::MapEigen::Quaterniond new_quat(x_plus_delta3); new_trans trans delta_trans; new_quat (delta_quat * quat).normalized(); return true; } virtual int GlobalSize() const { return 7; } virtual int LocalSize() const { return 6; } };3. G2O的图优化之道3.1 G2O的图结构设计G2O的核心优势在于其完整的图优化框架。一个典型的位姿图优化包含以下要素// 定义优化器 g2o::SparseOptimizer optimizer; optimizer.setVerbose(true); // 1. 创建线性求解器 auto linearSolver g2o::make_uniqueg2o::LinearSolverCholmodg2o::BlockSolverX::PoseMatrixType(); // 2. 创建块求解器 auto blockSolver g2o::make_uniqueg2o::BlockSolverX(std::move(linearSolver)); // 3. 选择优化算法 g2o::OptimizationAlgorithmLevenberg* algorithm new g2o::OptimizationAlgorithmLevenberg(std::move(blockSolver)); optimizer.setAlgorithm(algorithm);3.2 顶点与边的定义在G2O中我们需要明确定义顶点和边的类型。以3D位姿为例// 定义顶点 class VertexSE3 : public g2o::BaseVertex6, SE3 { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW virtual void setToOriginImpl() { _estimate SE3(); } virtual void oplusImpl(const double* update) { Eigen::Mapconst Vector6d v(update); _estimate SE3::exp(v) * _estimate; } virtual bool read(std::istream) { return false; } virtual bool write(std::ostream) const { return false; } }; // 定义边 class EdgeSE3 : public g2o::BaseBinaryEdge6, SE3, VertexSE3, VertexSE3 { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgeSE3() : g2o::BaseBinaryEdge6, SE3, VertexSE3, VertexSE3() {} void computeError() override { const VertexSE3* v1 static_castconst VertexSE3*(_vertices[0]); const VertexSE3* v2 static_castconst VertexSE3*(_vertices[1]); _error (_measurement.inverse() * v1-estimate().inverse() * v2-estimate()).log(); } void setInformation(const Eigen::Matrixdouble,6,6 information) { _information information; } };3.3 信息矩阵的妙用G2O直接支持信息矩阵的设置这使得传感器不确定性的建模变得直观EdgeSE3* edge new EdgeSE3(); edge-setVertex(0, dynamic_castVertexSE3*(optimizer.vertex(pose1_id))); edge-setVertex(1, dynamic_castVertexSE3*(optimizer.vertex(pose2_id))); edge-setMeasurement(relative_pose); // 设置信息矩阵协方差矩阵的逆 Eigen::Matrixdouble,6,6 info ...; edge-setInformation(info); optimizer.addEdge(edge);4. GTSAM的因子图哲学4.1 因子图的基本概念GTSAM采用因子图Factor Graph模型将优化问题表示为概率图模型。一个典型的位姿图优化可以表示为// 创建因子图 NonlinearFactorGraph graph; // 添加先验因子 noiseModel::Diagonal::shared_ptr priorNoise noiseModel::Diagonal::Sigmas( (Vector(6) 0.1, 0.1, 0.1, 0.05, 0.05, 0.05).finished()); graph.add(PriorFactorPose3(0, Pose3(), priorNoise)); // 添加里程计因子 noiseModel::Diagonal::shared_ptr odometryNoise noiseModel::Diagonal::Sigmas( (Vector(6) 0.2, 0.2, 0.2, 0.1, 0.1, 0.1).finished()); graph.add(BetweenFactorPose3(0, 1, Pose3(Rot3(), Point3(1,0,0)), odometryNoise));4.2 增量式优化实现GTSAM的增量式优化特性使其特别适合SLAM应用// 创建ISAM2优化器 ISAM2Params parameters; parameters.relinearizeThreshold 0.01; parameters.relinearizeSkip 1; ISAM2 isam(parameters); // 初始估计 Values initialEstimate; initialEstimate.insert(0, Pose3()); initialEstimate.insert(1, Pose3(Rot3(), Point3(1.1,0.1,0))); // 增量更新 isam.update(graph, initialEstimate); isam.update(); // 执行优化 // 获取最新结果 Values currentEstimate isam.calculateEstimate();4.3 流形优化的优雅实现GTSAM内置了对各种流形的支持例如SO(3)和SE(3)// 定义3D位姿因子 BetweenFactorPose3 factor( x1, x2, Pose3(Rot3::RzRyRx(0,0,M_PI/2), Point3(1,0,0)), noiseModel::Isotropic::Sigma(6, 0.1)); // 自动处理流形运算 Values values; values.insert(x1, Pose3()); values.insert(x2, Pose3(Rot3::RzRyRx(0,0,M_PI/2.1), Point3(1.1,0.1,0))); // 优化会自动在流形上进行 GaussNewtonOptimizer optimizer(graph, values); Values result optimizer.optimize();5. 三大库的横向对比与选型建议5.1 性能对比实测数据通过实际测试Intel i7-11800HUbuntu 20.04我们得到以下基准测试结果测试场景Ceres(ms)G2O(ms)GTSAM(ms)10节点2D优化12.38.715.2100节点3D优化145.692.4118.7增量式优化(100步)不适用部分支持78.35.2 典型应用场景推荐根据实际项目经验我的选型建议如下快速原型开发优先选择Ceres接口简单直接文档完善调试方便传统SLAM系统G2O是稳妥选择专门为SLAM优化丰富的顶点/边类型社区支持良好增量式SLAMGTSAM优势明显内置iSAM2贝叶斯网络支持学术研究友好5.3 混合使用策略在一些复杂项目中我经常采用混合使用策略前端使用Ceres进行快速局部优化后端采用GTSAM进行全局优化关键帧优化使用G2O这种组合既能保证实时性又能获得良好的全局一致性。例如在无人机SLAM系统中Ceres处理视觉里程计GTSAM处理GPS融合取得了不错的效果。