1. g2o框架与BA优化基础g2oGeneral Graph Optimization是一个基于图优化的C库专门用于解决非线性最小二乘问题。在视觉SLAM和三维重建领域Bundle AdjustmentBA是最典型的应用场景之一。BA的核心思想是通过最小化重投影误差来优化相机位姿和三维点坐标。图优化将BA问题建模为一个图结构顶点Vertex需要优化的变量包括相机位姿SE3和三维点坐标PointXYZ边Edge表示观测约束即重投影误差项在g2o中实现BA需要配置几个关键组件线性求解器LinearSolver处理海森矩阵的求解常用Eigen或CSparse块求解器BlockSolver管理变量块的求解策略优化算法OptimizationAlgorithm通常选择Levenberg-MarquardtLM算法提示g2o的模板参数6,3表示相机位姿是6自由度SE3三维点是3自由度。这个配置需要与实际问题维度严格匹配。2. BA优化实现详解2.1 优化器初始化配置优化器的初始化是BA的基础需要正确配置求解器和算法g2o::SparseOptimizer optimizer; // 使用Eigen作为线性求解器 auto linearSolver new g2o::LinearSolverEigeng2o::BlockSolver_6_3::PoseMatrixType(); // 创建块求解器 auto solver_ptr new g2o::BlockSolver_6_3(linearSolver); // 使用LM算法 auto solver new g2o::OptimizationAlgorithmLevenberg(solver_ptr); optimizer.setAlgorithm(solver);这里有几个关键选择线性求解器选择Eigen而非CSparse因为Eigen是纯头文件库无需额外依赖对于中小规模问题Eigen性能足够避免了CSparse的LGPL协议限制LM算法相比Gauss-Newton增加了阻尼因子更稳定能自适应调整步长适合SLAM这类非凸优化问题2.2 顶点添加与配置BA需要添加两类顶点相机位姿和三维点。相机位姿顶点VertexSE3Expmapg2o::VertexSE3Expmap* vSE3 new g2o::VertexSE3Expmap(); vSE3-setEstimate(Converter::toSE3Quat(pKF-GetPose())); vSE3-setId(pKF-mnId); vSE3-setFixed(pKF-mnId0); // 第一帧固定 optimizer.addVertex(vSE3);关键细节使用李代数表示旋转避免万向锁问题第一帧通常固定提供绝对参考系ID分配要唯一且连续影响求解效率三维点顶点VertexSBAPointXYZg2o::VertexSBAPointXYZ* vPoint new g2o::VertexSBAPointXYZ(); vPoint-setEstimate(Converter::toVector3d(pMP-GetWorldPos())); vPoint-setId(pMP-mnIdmaxKFid1); vPoint-setMarginalized(true); // 启用边缘化 optimizer.addVertex(vPoint);边缘化Marginalization的作用将点坐标变量从海森矩阵中消去显著降低求解复杂度特别适合点数量远多于相机的情况2.3 边误差项的构建重投影误差边是BA的核心约束条件分为单目和双目两种情况。单目边EdgeSE3ProjectXYZEigen::Matrixdouble,2,1 obs; obs kpUn.pt.x, kpUn.pt.y; auto e new g2o::EdgeSE3ProjectXYZ(); e-setVertex(0, dynamic_castg2o::OptimizableGraph::Vertex*(optimizer.vertex(id))); e-setVertex(1, dynamic_castg2o::OptimizableGraph::Vertex*(optimizer.vertex(pKF-mnId))); e-setMeasurement(obs); e-setInformation(Eigen::Matrix2d::Identity()*invSigma2);双目边EdgeStereoSE3ProjectXYZEigen::Matrixdouble,3,1 obs; obs kpUn.pt.x, kpUn.pt.y, kp_ur; auto e new g2o::EdgeStereoSE3ProjectXYZ(); e-setVertex(0, dynamic_castg2o::OptimizableGraph::Vertex*(optimizer.vertex(id))); e-setVertex(1, dynamic_castg2o::OptimizableGraph::Vertex*(optimizer.vertex(pKF-mnId))); e-setMeasurement(obs); e-setInformation(Eigen::Matrix3d::Identity()*invSigma2);信息矩阵Information Matrix的设置与特征点尺度octave相关高层金字塔的特征点赋予更低权重反映观测的不确定性2.4 鲁棒核函数应用为处理异常观测需要添加鲁棒核函数if(bRobust) { auto rk new g2o::RobustKernelHuber; e-setRobustKernel(rk); rk-setDelta(mono ? sqrt(5.99) : sqrt(7.815)); }鲁棒核的作用降低外点outliers的影响Huber核是L1-L2的折中保持可微性Delta值根据卡方分布确定95%置信度3. 优化过程与结果处理3.1 优化执行optimizer.initializeOptimization(); optimizer.optimize(nIterations);迭代次数nIterations的选择局部BA10-20次通常足够全局BA可能需要50-100次实际中可根据误差下降情况动态调整3.2 优化结果提取相机位姿更新g2o::SE3Quat SE3quat vSE3-estimate(); pKF-SetPose(Converter::toCvMat(SE3quat));三维点更新pMP-SetWorldPos(Converter::toCvMat(vPoint-estimate())); pMP-UpdateNormalAndDepth();注意区分局部BA和全局BA局部BA直接更新位姿和点全局BA需要暂存结果mTcwGBA/mPosGBA避免在回环检测过程中频繁更新4. 实战经验与性能优化4.1 常见问题排查优化发散检查初始值是否合理降低LM算法的初始阻尼因子增加鲁棒核的delta值求解速度慢使用更好的线性求解器如SuiteSparse减少边缘化点的数量尝试PCG迭代求解器内存不足限制参与BA的关键帧数量使用Schur补加速求解考虑增量式BA4.2 性能优化技巧观测筛选剔除重投影误差过大的边按金字塔层级加权限制每个点的最大观测数并行化使用g2o的多线程优化将BA任务分配到多个线程注意线程安全的数据访问内存管理重用优化器对象预分配顶点和边内存使用内存池技术4.3 参数调优建议LM算法参数初始lambda1e-6lambda因子10最大lambda1e10鲁棒核参数单目deltasqrt(5.99)双目deltasqrt(7.815)动态调整策略迭代控制绝对误差阈值1e-6相对误差阈值1e-6最大迭代时间500ms在实际SLAM系统中BA优化通常占用了大部分计算资源。通过合理配置g2o和优化BA策略可以显著提升系统性能。建议从局部BA开始调试逐步扩展到全局BA同时注意监控优化过程中误差的变化情况。
