SLAM核心算法之G2O优化器_耀世娱乐全球认证招商站

常见SLAM算法优化器一般为G2O和Ceres，其中G2O封装的接口简单、易于理解，在实际应用中也较为广泛。G2O核心设计为图优化的方式，将帧的位姿pose和可视3D点云作为顶点，利用重投影误差连接顶点（目标函数即为重投影误差），通常使用列文伯格等求解器进行迭代优化顶点。

G2O使用原理解析：

相机位姿为vertex （对应顶点编号0和1）

2. 将第一帧的特征点转换为3D点云，并且添加为vertex（对应顶点编号2->n）

3. 根据两帧的特征点依次添加edge，优化器优化目标使用重投影误差，代码中edge->setVertex表示手动指定edge的顶点序列编号，setVertex之后，即完成了边和顶点的连接。

（注：G2O的jacbian已经由G2O内部实现，也可对应3D计算机视觉教材进行查找和学习该雅可比矩阵，主要是通过残差对状态变量求偏导实现）

以下为调试完成可执行的G2O测试代码，pts1和pts2为对图像抽取的特征点，可使用opencv特征提取函数得到

    LOGI("g2o 找到了%d 组对应特征点。", pts1.size());
    // 构造g2o中的图
    // 先构造求解器
    g2o::SparseOptimizer    optimizer;
    // 使用Cholmod中的线性方程求解器
    g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new  g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType> ();
    // 6*3 的参数
    g2o::BlockSolver_6_3* block_solver = new g2o::BlockSolver_6_3( linearSolver );
    // L-M 下降
    g2o::OptimizationAlgorithmLevenberg* algorithm = new g2o::OptimizationAlgorithmLevenberg( block_solver );

    optimizer.setAlgorithm( algorithm );
    optimizer.setVerbose( false );

    // 添加节点
    // 两个位姿节点
    for ( int i=0; i<2; i++ )
    {
        g2o::VertexSE3Expmap* v = new g2o::VertexSE3Expmap();
        v->setId(i);
        if ( i == 0)
            v->setFixed( true ); // 第一个点固定为零
        // 预设值为单位Pose，因为我们不知道任何信息
        v->setEstimate( g2o::SE3Quat() );
        optimizer.addVertex( v );
    }
    // 很多个特征点的节点
    // 以第一帧为准
    for ( size_t i=0; i<pts1.size(); i++ )
    {
        g2o::VertexSBAPointXYZ* v = new g2o::VertexSBAPointXYZ();
        v->setId( 2 + i );
        // 由于深度不知道，只能把深度设置为1了
        double z = 1;
        double x = ( pts1[i].pt.x - cx ) * z / fx;
        double y = ( pts1[i].pt.y - cy ) * z / fy;
        v->setMarginalized(true);
        v->setEstimate( Eigen::Vector3d(x,y,z) );
        optimizer.addVertex( v );
    }

//    g2o::CameraParameters* camera=new g2o::CameraParameters( fx, Eigen::Vector2d(cx, cy), 0 );
//    camera->setId(0);
//    optimizer.addParameter( camera );

    // 准备边
    // 第一帧
    vector<ORB_SLAM3::EdgeSE3ProjectXYZ*> edges;
    for ( size_t i=0; i<pts1.size(); i++ )
    {
        // v1表示mappoint的3D坐标, v2表示相机位姿
        g2o::EdgeSE3ProjectXYZ*  edge = new g2o::EdgeSE3ProjectXYZ();
        edge->setVertex( 0, dynamic_cast<g2o::OptimizableGraph::Vertex*>   (optimizer.vertex(i+2)) );
        edge->setVertex( 1, dynamic_cast<g2o::OptimizableGraph::Vertex*>     (optimizer.vertex(0)) );
        edge->setMeasurement( Eigen::Vector2d(pts1_un[i].pt.x, pts1_un[i].pt.y ) );
        const float &invSigma2 = mvInvLevelSigma2[pts1_un[i].octave];
        edge->setInformation( Eigen::Matrix2d::Identity() *invSigma2);
        edge->setParameterId(0, 0);
        // 核函数
        edge->setRobustKernel( new g2o::RobustKernelHuber() );
        edge->fx = fx;
        edge->fy = fy;
        edge->cx = cx;
        edge->cy = cy;
        optimizer.addEdge( edge );
        edges.push_back(edge);
    }
    // 第二帧
    for ( size_t i=0; i<pts2.size(); i++ )
    {
        g2o::EdgeSE3ProjectXYZ*  edge = new g2o::EdgeSE3ProjectXYZ();
        edge->setVertex( 0, dynamic_cast<g2o::OptimizableGraph::Vertex*>   (optimizer.vertex(i+2)) );
        edge->setVertex( 1, dynamic_cast<g2o::OptimizableGraph::Vertex*>     (optimizer.vertex(1)) );
        edge->setMeasurement( Eigen::Vector2d(pts2_un[i].pt.x, pts2_un[i].pt.y ) );
        const float &invSigma2 = mvInvLevelSigma2[pts2_un[i].octave];
        edge->setInformation( Eigen::Matrix2d::Identity() *invSigma2);
        edge->setParameterId(0,0);
        // 核函数
        edge->setRobustKernel( new g2o::RobustKernelHuber() );
        edge->fx = fx;
        edge->fy = fy;
        edge->cx = cx;
        edge->cy = cy;
        optimizer.addEdge( edge );
        edges.push_back(edge);
    }

    LOGI("g2o 开始优化");
    optimizer.setVerbose(true);
    optimizer.initializeOptimization();
    optimizer.optimize(100);
    LOGI("g2o 优化完毕");

    //两帧之间的变换矩阵
    g2o::VertexSE3Expmap* v = dynamic_cast<g2o::VertexSE3Expmap*>( optimizer.vertex(1) );
    Eigen::Isometry3d pose = v->estimate();

    // 以及所有特征点的位置
    for ( size_t i=0; i<pts1.size(); i++ )
    {
        g2o::VertexSBAPointXYZ* v = dynamic_cast<g2o::VertexSBAPointXYZ*> (optimizer.vertex(i+2));
        Eigen::Vector3d pos = v->estimate();
        // LOGI("g2o vertex id %d, pos=%f, %f, %f", i+2, pos(0), pos(1),pos(2));
    }

    // 估计inlier的个数
    int inliers = 0;
    for ( auto e:edges )
    {
        e->computeError();
        // chi2 就是 error*\\Omega*error, 如果这个数很大，说明此边的值与其他边很不相符
        if ( e->chi2() > 1 )
        {
            // LOGI("g2o error=%f", e->chi2());
        }
        else
        {
            inliers++;
        }
    }
    LOGI("g2o inliers=%d", inliers);