g2o使用总结[亲测有效]

大家好，欢迎来到IT知识分享网。

需要解决的问题：
如何自定义Vertex和Edge？
如何选择Edge类型？一元还是二元？
如何赋值信息矩阵？
如何设置鲁棒核函数的阈值？
如何选择Vertex设置为固定？
如何边缘化以便稀疏化求解？
如何处理优化结束后outliner？
如何设置根据卡方分布的临界值表对Edge的chi2设置阈值？

1. g2o提供的顶点vertex

1） 李代数位姿

class VertexSE3Expmap : public BaseVertex<6, SE3Quat>

继承于BaseVertex这个模板类
需要设置的模板参数：

• 参数6 ：SE3Quat类型为六维，三维旋转，三维平移
• 参数SE3Quat ：该类型旋转在前，平移在后，注意：类型内部使用的其实是四元数，不是李代数

该顶点需要设置的参数：

g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
//【1】设置待优化位姿（这是粗略位姿）
vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));
//【2】设置Id号
vSE3->setId(pKFi->mnId);
//【3】设置是否固定，第一帧固定
vSE3->setFixed(pKFi->mnId==0);

2） 空间点位置

class VertexSBAPointXYZ : public BaseVertex<3, Vector3d>

该顶点需要设置的参数：

g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
//【1】设置待优化空间点3D位置XYZ
vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
//【2】设置Id号
vPoint->setId(id);
//【3】是否边缘化（以便稀疏化求解）
vPoint->setMarginalized(true);

2. g2o提供的边edge

1） Point-Pose 二元边（$Point_{XYZ}$-SE3边）
即要优化MapPoints的位置，又要优化相机的位姿

class EdgeSE3ProjectXYZ: public BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap>

继承于BaseBinaryEdge这个二元边模板类
需要设置的模板参数：

• 参数2 ：观测值（这里是3D点在像素坐标系下的投影坐标）的维度
• 参数Vector ：观测值类型，piexl.x，piexl.y
• 参数VertexSBAPointXYZ：第一个顶点类型
• 参数VertexSE3Expmap ：第二个顶点类型

该边需要设置的参数：

g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();

//【1】设置第一个顶点，注意该顶点类型与模板参数第一个顶点类型对应
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
//【2】设置第二个顶点
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFi->mnId)));
//【3】设置观测值，类型与模板参数对应
e->setMeasurement(obs);
const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];
//【4】设置信息矩阵，协方差
e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);

//【5】设置鲁棒核函数
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
rk->setDelta(thHuberMono);

//【6】设置相机内参
e->fx = pKFi->fx;
e->fy = pKFi->fy;
e->cx = pKFi->cx;
e->cy = pKFi->cy;

2） Pose 一元边（SE3）
仅优化相机位姿，为了构造出投影方程，需要按下面的方式把MapPoints的位置作为常量加入

class EdgeSE3ProjectXYZOnlyPose: public BaseUnaryEdge<2, Vector2d, VertexSE3Expmap>

该继承于BaseUnaryEdge这个一元边模板类，需要设置的模板参数如上

该边需要设置的参数：

g2o::EdgeSE3ProjectXYZOnlyPose* e = new g2o::EdgeSE3ProjectXYZOnlyPose();

// 注意这里只设置一个顶点，其它一样
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
e->setMeasurement(obs);
const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];
e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);

g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
rk->setDelta(deltaMono); /** @attention 设置阈值，卡方自由度为2，内点概率95%对应的临界值*/

e->fx = pFrame->fx;
e->fy = pFrame->fy;
e->cx = pFrame->cx;
e->cy = pFrame->cy;

/** @attention 需要在这里设置<不做优化>的MapPoints的位置*/
cv::Mat Xw = pMP->GetWorldPos();
e->Xw[0] = Xw.at<float>(0);
e->Xw[1] = Xw.at<float>(1);
e->Xw[2] = Xw.at<float>(2);

3） Pose-Pose 二元边（SE3-SE3边）
优化变量是相机相邻两个关键帧位姿，约束来自对这两个关键帧位姿变换的测量（里程计、IMU等）

class G2O_TYPES_SBA_API EdgeSE3Expmap : public BaseBinaryEdge<6, SE3Quat, VertexSE3Expmap, VertexSE3Expmap>

需要设置的参数如下：

Se2 measure_se2 = pMsrOdo->se2;
// 【1】里程计测量的协方差
g2o::Matrix3D covariance = toEigenMatrixXd(pMsrOdo->info).inverse(); 

// 【2】将里程计测量转换成g2o::SE3Quat类型
Eigen::AngleAxisd rotz(measure_se2.theta, Eigen::Vector3d::UnitZ());
g2o::SE3Quat relativePose_SE3Quat(rotz.toRotationMatrix(), Eigen::Vector3d(measure_se2.x, measure_se2.y, 0));

// 【3】将`里程计测量协方差`转换为`相机坐标系下协方差`
// 注意：g2o::SE3Quat是旋转在前，平移在后
g2o::Matrix6d covariance_6d = g2o::Matrix6d::Identity();
covariance_6d(0,0) = covariance(2,2);
covariance_6d(0,4) = covariance(2,0); covariance_6d(0,5) = covariance(2,1);
covariance_6d(4,0) = covariance(0,2); covariance_6d(5,0) = covariance(1,2);
covariance_6d(3,3) = covariance(0,0);
covariance_6d(4,4) = covariance(1,1);
covariance_6d(1,1) = 0.00001;
covariance_6d(2,2) = 0.01;
covariance_6d(5,5) = 0.0001;

g2o::Matrix6d Info = g2o::Matrix6d::Identity();
Info = covariance_6d.inverse();

// 【4】构造边
g2o::EdgeOnlineCalibration* e = new g2o::EdgeOnlineCalibration;
e->vertices()[0] = optimizer.vertex(id0); e->vertices()[1] = optimizer.vertex(id1); e->setMeasurement(relativePose_SE3Quat); e->setInformation(Info); optimizer.addEdge(e);

上面的比较麻烦的协方差的计算，准确的协方差计算可以参考论文《Odometry-Vision-Based Ground Vehicle Motion Estimation With SE(2)-Constrained SE(3) Poses》中的处理.

3. g2o优化

//【1】指定pose维度为6, landmark维度为3
typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;
//【2】线性方程求解器，使用CSparse分解
// 还有g2o::LinearSolverDense，使用cholesky分解
Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); 
//【3】矩阵块求解器
Block* solver_ptr = new Block ( linearSolver );     
//【4】梯度下降方法，从GN, LM, DogLeg 中选
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
// 图模型
g2o::SparseOptimizer optimizer;
//【5】设置求解器
optimizer.setAlgorithm ( solver );
......
......
......
//【6】打开调试输出
optimizer.setVerbose ( true );
optimizer.initializeOptimization();
//【7】指定迭代次数:100次
optimizer.optimize ( 100 );

g2o中经常使用的BlockSolver_6_3、BlockSolver_7_3 即是上面程序中【1】所作的事，如下：

  // solver for BA/3D SLAM
  typedef BlockSolver< BlockSolverTraits<6, 3> > BlockSolver_6_3;  
  // solver fo BA with scale
  typedef BlockSolver< BlockSolverTraits<7, 3> > BlockSolver_7_3; 

4. 检测outliner

优化完成后，对每一条边都进行检查，剔除误差较大的边（认为是错误的边），并设置setLevel为0，即下次不再对该边进行优化
第二次优化完成后，会对连接偏差比较大的边，在关键帧中剔除对该MapPoint的观测，在MapPoint中剔除对该关键帧的观测，具体实现参考orb-slam源码Optimizer::LocalBundleAdjustment

optimizer.optimize ( 100 );
// 优化完成后，进行Edge的检查
for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++)
{
    g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
    MapPoint* pMP = vpMapPointEdgeMono[i];

    if(pMP->isBad())
        continue;

    // 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）
    // 第二个判断条件，用于检查构成该边的MapPoint在该相机坐标系下的深度是否为正？
    if(e->chi2()>5.991 || !e->isDepthPositive())
    {
        e->setLevel(1);// 不优化
    }

    // 因为剔除了错误的边，所以下次优化不再使用核函数
    e->setRobustKernel(0);
}

5. 加入对相机内参的优化

g2o::CameraParameters* camera = new g2o::CameraParameters (
    K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0
);
// 设置Id号为0即可
camera->setId ( 0 );
optimizer.addParameter ( camera );

如果不想优化相机内参，则内参按照第二步中二元边中的程序demo中设置

6. 实践：自定义PoseGraphy优化

g2o/types/slam3d/中的SE3位姿，它实质上使用的是四元数而非李代数。接下来使用Sophus表示的李代数来构造g2o中顶点和边。

自定义g2o用到的顶点和边
Vertex：关键帧的位姿
Edge：测量出来的两个关键帧位姿节点之间的相对运动$\Delta$$T_{ij}$，可以来自wheel odometry、或者IMU、或者orb-slam中Optimizer::OptimizeEssentialGraph函数中的方式

1）自定义李代数顶点

class VertexSE3LieAlgebra : public g2o::BaseVertex<6, SE3>
{
......
}

VertexSE3LieAlgebra是自定义的类名，继承于g2o::BaseVertex
两个模板参数分别表示：

• 参数 6：顶点的维度，这里是Sopuse的SE3的维度
• 参数 SE3：顶点的类型，这里是Sopuse的SE3

2）自定义两个李代数顶点之边

class EdgeSE3LieAlgebra: public g2o::BaseBinaryEdge<6, SE3, VertexSE3LieAlgebra, VertexSE3LieAlgebra>
{
......
}

EdgeSE3LieAlgebra是自定义的类名，继承于g2o::BaseBinaryEdge模板类（二元边）
四个模板参数分别表示：

• 参数 6：观测值的维度，这里是第二个参数SE3的维度
• 参数 SE3：观测值的类型，这里是Sopuse的SE3，两个位姿节点之间的相对运动$\Delta$$T_{ij}$，即相邻两关键帧之间的位姿变化
• 参数 VertexSE3LieAlgebra： 第一个顶点的类型，上一步自定义的关于相机位姿的顶点
• 参数 VertexSE3LieAlgebra： 第二个顶点的类型

<完>
@LeatherWang