g2o框架下的BA优化原理与实现详解

1. g2o框架与BA优化基础

g2o（General Graph Optimization）是一个基于图优化的C++库，专门用于解决非线性最小二乘问题。在视觉SLAM和三维重建领域，Bundle Adjustment（BA）是最典型的应用场景之一。BA的核心思想是通过最小化重投影误差来优化相机位姿和三维点坐标。

图优化将BA问题建模为一个图结构：

• 顶点（Vertex）：需要优化的变量，包括相机位姿（SE3）和三维点坐标（PointXYZ）
• 边（Edge）：表示观测约束，即重投影误差项

在g2o中实现BA需要配置几个关键组件：

1. 线性求解器（LinearSolver）：处理海森矩阵的求解，常用Eigen或CSparse
2. 块求解器（BlockSolver）：管理变量块的求解策略
3. 优化算法（OptimizationAlgorithm）：通常选择Levenberg-Marquardt（LM）算法

提示：g2o的模板参数 <6,3> 表示相机位姿是6自由度（SE3），三维点是3自由度。这个配置需要与实际问题维度严格匹配。

2. BA优化实现详解

2.1 优化器初始化配置

优化器的初始化是BA的基础，需要正确配置求解器和算法：

g2o::SparseOptimizer optimizer;
// 使用Eigen作为线性求解器
auto linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
// 创建块求解器
auto solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
// 使用LM算法
auto solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);

这里有几个关键选择：

1. 线性求解器选择Eigen而非CSparse，因为：

   • Eigen是纯头文件库，无需额外依赖
   • 对于中小规模问题，Eigen性能足够
   • 避免了CSparse的LGPL协议限制

2. LM算法相比Gauss-Newton：

   • 增加了阻尼因子，更稳定
   • 能自适应调整步长
   • 适合SLAM这类非凸优化问题

2.2 顶点添加与配置

BA需要添加两类顶点：相机位姿和三维点。

相机位姿顶点（VertexSE3Expmap） ：

g2o::VertexSE3Expmap* vSE3 = new g2o::VertexSE3Expmap();
vSE3->setEstimate(Converter::toSE3Quat(pKF->GetPose()));
vSE3->setId(pKF->mnId);
vSE3->setFixed(pKF->mnId==0);  // 第一帧固定
optimizer.addVertex(vSE3);

关键细节：

• 使用李代数表示旋转，避免万向锁问题
• 第一帧通常固定，提供绝对参考系
• ID分配要唯一且连续，影响求解效率

三维点顶点（VertexSBAPointXYZ） ：

g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
vPoint->setId(pMP->mnId+maxKFid+1);
vPoint->setMarginalized(true);  // 启用边缘化
optimizer.addVertex(vPoint);

边缘化（Marginalization）的作用：

• 将点坐标变量从海森矩阵中消去
• 显著降低求解复杂度
• 特别适合点数量远多于相机的情况

2.3 边（误差项）的构建

重投影误差边是BA的核心约束条件，分为单目和双目两种情况。

单目边（EdgeSE3ProjectXYZ） ：

Eigen::Matrix<double,2,1> obs;
obs << kpUn.pt.x, kpUn.pt.y;

auto e = new g2o::EdgeSE3ProjectXYZ();
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKF->mnId)));
e->setMeasurement(obs);
e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);

双目边（EdgeStereoSE3ProjectXYZ） ：

Eigen::Matrix<double,3,1> obs;
obs << kpUn.pt.x, kpUn.pt.y, kp_ur;

auto e = new g2o::EdgeStereoSE3ProjectXYZ();
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
e->setVertex(1, dynamic_cast<g2o::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的选择：

• 局部BA：10-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 常见问题排查

1. 优化发散 ：

   • 检查初始值是否合理
   • 降低LM算法的初始阻尼因子
   • 增加鲁棒核的delta值

2. 求解速度慢 ：

   • 使用更好的线性求解器（如SuiteSparse）
   • 减少边缘化点的数量
   • 尝试PCG迭代求解器

3. 内存不足 ：

   • 限制参与BA的关键帧数量
   • 使用Schur补加速求解
   • 考虑增量式BA

4.2 性能优化技巧

1. 观测筛选 ：

   • 剔除重投影误差过大的边
   • 按金字塔层级加权
   • 限制每个点的最大观测数

2. 并行化 ：

   • 使用g2o的多线程优化
   • 将BA任务分配到多个线程
   • 注意线程安全的数据访问

3. 内存管理 ：

   • 重用优化器对象
   • 预分配顶点和边内存
   • 使用内存池技术

4.3 参数调优建议

1. LM算法参数：

   • 初始lambda：1e-6
   • lambda因子：10
   • 最大lambda：1e10

2. 鲁棒核参数：

   • 单目：delta=sqrt(5.99)
   • 双目：delta=sqrt(7.815)
   • 动态调整策略

3. 迭代控制：

   • 绝对误差阈值：1e-6
   • 相对误差阈值：1e-6
   • 最大迭代时间：500ms

在实际SLAM系统中，BA优化通常占用了大部分计算资源。通过合理配置g2o和优化BA策略，可以显著提升系统性能。建议从局部BA开始调试，逐步扩展到全局BA，同时注意监控优化过程中误差的变化情况。