《SLAM14讲》后端知识汇总及非线性优化

1、介绍一些你熟悉的非线性优化库

        非线性优化库一般有ceres和g2o两种，我比较熟悉的是g2o，看下g2o的结构：

        它表示了g2o中的类结构。 首先根据前面的代码经验可以发现，我们最终使用的optimizer是一个SparseOptimizer对象，因此我们要维护的就是它(对它进行各种操作)。 一个SparseOptimizer是一个可优化图(OptimizableGraph)，也是一个超图(HyperGraph)。而图中有很多顶点(Vertex)和边(Edge)。顶点继承于BaseVertex，边继承于BaseUnaryEdge、BaseBinaryEdge或BaseMultiEdge。它们都是抽象的基类，实际有用的顶点和边都是它们的派生类。我们用SparseOptimizer.addVertex和SparseOptimizer.addEdge向一个图中添加顶点和边，最后调用SparseOptimizer.optimize完成优化。
 

在优化之前还需要制定求解器和迭代算法。一个SparseOptimizer拥有一个OptimizationAlgorithm，它继承自Gauss-Newton, Levernberg-Marquardt, Powell’s dogleg三者之一。同时，这个OptimizationAlgorithm拥有一个Solver，它含有两个部分。一个是 SparseBlockMatrix，用于计算稀疏的雅可比和海塞矩阵；一个是线性方程求解器，可从PCG、CSparse、Choldmod三选一，用于求解迭代过程中最关键的一步：
H Δ x = − b 

        因此理清了g2o的结构，也就知道了其使用流程。在之前已经说过了，这里就再重复一遍：
（1）选择一个线性方程求解器，PCG、CSparse、Choldmod三选一，来自g2o/solvers文件夹
（2）选择一个BlockSolver，用于求解雅克比和海塞矩阵，来自g2o/core文件夹
（3）选择一个迭代算法，GN、LM、DogLeg三选一，来自g2o/core文件夹
参考G2O图优化基础和SLAM的Bundle Adjustment(光束法平差)

 代码如下：

void bundleAdjustment (
        const vector< Point3f > points_3d,
        const vector< Point2f > points_2d,
        Mat& K   )
{
    // creat g2o
    // new g2o version. Ref:https://www.cnblogs.com/xueyuanaichiyu/p/7921382.html

    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6, 3> > Block;  // pose 维度为 6, landmark 维度为 3
    // 第1步：创建一个线性求解器LinearSolver
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>();
    // 第2步：创建BlockSolver。并用上面定义的线性求解器初始化
    Block* solver_ptr = new Block (  std::unique_ptr<Block::LinearSolverType>(linearSolver) );
    // Block* solver_ptr = new Block ( linearSolver );

    // 第3步：创建总求解器solver。并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( std::unique_ptr<Block>(solver_ptr) );
    // g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );

    // 第4步：创建稀疏优化器
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm ( solver );

//    // old g2o version
//    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  // pose 维度为 6, landmark 维度为 3
//    Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); // 线性方程求解器
//    Block* solver_ptr = new Block ( linearSolver );     // 矩阵块求解器
//    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
//    g2o::SparseOptimizer optimizer;
//    optimizer.setAlgorithm ( solver );

    // 第5步：定义图的顶点和边。并添加到SparseOptimizer中
	// ----------------------开始你的代码：设置并添加顶点，初始位姿为单位矩阵
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();    // camera pose， 李代数位姿顶点
    Eigen::Matrix3d R_mat = Eigen::Matrix3d::Identity();        // 设置为单位阵
    pose->setId(0);
    //pose->setEstimate(g2o::SE3Quat(
    //                        R_mat,
    //                        Eigen::Vector3d::Identity()));    // 这里初值设置错误
    pose->setEstimate(g2o::SE3Quat(
            R_mat,
            Eigen::Vector3d::Zero()));                          // 平移部分设置为0
    optimizer.addVertex( pose );

    int index = 1;
    for ( const Point3f p : points_3d )   // landmarks，第一幅图坐标系下的三维空间点
    {
        g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
        point->setId( index ++ );
        point->setEstimate( Eigen::Vector3d( p.x, p.y, p.z ) );
        point->setMarginalized( true );     // g2o中必须设置marg参见第10讲内容
        optimizer.addVertex( point );
    }
	// ----------------------结束你的代码
	
	
    // 设置相机内参
    g2o::CameraParameters* camera = new g2o::CameraParameters (
            K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0);
    camera->setId ( 0 );
    optimizer.addParameter ( camera );

    // 设置边
    index = 1;
    for ( const Point2f p:points_2d )
    {
        g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
        edge->setId ( index );
        edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );
        edge->setVertex ( 1, pose );
        edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );  //设置观测值
        edge->setParameterId ( 0,0 );
        edge->setInformation ( Eigen::Matrix2d::Identity() );
        optimizer.addEdge ( edge );
        index ++;
    }

    // 第6步：设置优化参数，开始执行优化
    optimizer.setVerbose ( false );
    optimizer.initializeOptimization();
    optimizer.optimize ( 100 );

    // 输出优化结果
    cout<<endl<<"after optimization:"<<endl;
    cout<<"T="<<endl<<Eigen::Isometry3d ( pose->estimate() ).matrix() <<endl;
}

这里我补充下：
        注意到上面的结构图中，节点Basevertex<D,T>，BaseBinaryEdge<D,E,VertexXi,VertexXj>和BlockSolver<>等都是模板类，我们可以根据自己的需要初始化不同类型的节点和边以及求解器，以ORB SLAM2为例，分析下后端最典型的全局BA所用的边、节点和求解器：
        （1）边是EdgeSE3ProjectXYZ，它其实就是继承自BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap>，其模板类型里第一个参数是观测值维度，这里的观测值是其实就是我们的像素误差u , v u,vu,v，第二个参数就是我们观测值的类型，第三个第四个就是我们边两头节点的类型；
        （2）相机节点VertexSE3Expmap，它其实就是继承自BaseVertex<6, SE3Quat>，其模板类第一个参数就是其维度，SE3是六维的这没毛病，第二个就是节点的类型，SE3Quat就是g2o自定义的SE3的类，类里面写了各种SE3的计算法则；
        （3）空间点节点VertexSBAPointXYZ，它其实就是继承自BaseVertex<3, Vector3d>，其模板类第一个参数是说明咱空间点的维度是三维，第二个参数说明这个点的类型是Vector3d；
        （4）求解器是BlockSolver_6_3，它其实就是BlockSolver< BlockSolverTraits<6, 3> >，6,3分别指代的就是边两边的维度了。

        我记得我刚开始学习SLAM的时候自己想办法写后端的时候很纳闷这个图是怎么构建起来的，在ORB或者SVO里面，所有的地图点和关键帧都是以类的形式存在的，例如在ORB中是先将关键帧的节点添加起来，然后添加空间点，然后遍历空间点中记录的与哪些关键帧有关系，然后相应ID的关键帧的节点和空间点的节点连接起来，然后就把图建立起来了，我觉得不写类好像没有什么其他更好的办法了。