RGBD-SLAM学习6

整个第六讲其实就是增加了g2o优化部分
在看g2o时，看到了贺大神的博文，优化部分的简单举例感觉非常好，非常有代入感的介绍了什么是优化，为什么要优化，如何操作等问题。其实更高维度的优化核心逻辑是一样的，只是操作起来复杂，需要更高级的操作。

http://blog.csdn.net/heyijia0327/article/details/47686523

转过来看高博的RGBD，先贴一下代码：

#include <iostream>
#include <fstream>
#include <sstream>
using namespace std;

#include "slamBase.h"

//g2o的头文件
#include <g2o/types/slam3d/types_slam3d.h> //顶点类型
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/factory.h>
#include <g2o/core/optimization_algorithm_factory.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/core/robust_kernel_factory.h>
#include <g2o/core/optimization_algorithm_levenberg.h>


// 给定index，读取一帧数据
FRAME readFrame( int index, ParameterReader& pd );
// 估计一个运动的大小
double normofTransform( cv::Mat rvec, cv::Mat tvec );

int main( int argc, char** argv )
{
    // 前面部分和vo是一样的
    ParameterReader pd;
    int startIndex  =   atoi( pd.getData( "start_index" ).c_str() );
    int endIndex    =   atoi( pd.getData( "end_index"   ).c_str() );

    // initialize
    cout<<"Initializing ..."<<endl;
    int currIndex = startIndex; // 当前索引为currIndex
    FRAME lastFrame = readFrame( currIndex, pd ); // 上一帧数据
    // 我们总是在比较currFrame和lastFrame
    string detector = pd.getData( "detector" );
    string descriptor = pd.getData( "descriptor" );
    CAMERA_INTRINSIC_PARAMETERS camera = getDefaultCamera();
    computeKeyPointsAndDesp( lastFrame, detector, descriptor );
    PointCloud::Ptr cloud = image2PointCloud( lastFrame.rgb, lastFrame.depth, camera );

    pcl::visualization::CloudViewer viewer("viewer");

    // 是否显示点云
    bool visualize = pd.getData("visualize_pointcloud")==string("yes");

    int min_inliers = atoi( pd.getData("min_inliers").c_str() );
    double max_norm = atof( pd.getData("max_norm").c_str() );

    /******************************* 
    // 新增:有关g2o的初始化
    *******************************/
    // 选择优化方法
    typedef g2o::BlockSolver_6_3 SlamBlockSolver; 
    typedef g2o::LinearSolverCSparse< SlamBlockSolver::PoseMatrixType > SlamLinearSolver; 

    // 初始化求解器
    SlamLinearSolver* linearSolver = new SlamLinearSolver();
    linearSolver->setBlockOrdering( false );
    SlamBlockSolver* blockSolver = new SlamBlockSolver( linearSolver );
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( blockSolver );

    g2o::SparseOptimizer globalOptimizer;  // 最后用的就是这个东东
    globalOptimizer.setAlgorithm( solver ); 
    // 不要输出调试信息
    globalOptimizer.setVerbose( false );

    // 向globalOptimizer增加第一个顶点
    g2o::VertexSE3* v = new g2o::VertexSE3();
    v->setId( currIndex );
    v->setEstimate( Eigen::Isometry3d::Identity() ); //估计为单位矩阵
    v->setFixed( true ); //第一个顶点固定，不用优化
    globalOptimizer.addVertex( v );

    int lastIndex = currIndex; // 上一帧的id

    for ( currIndex=startIndex+1; currIndex<endIndex; currIndex++ )
    {
        cout<<"Reading files "<<currIndex<<endl;
        FRAME currFrame = readFrame( currIndex,pd ); // 读取currFrame
        computeKeyPointsAndDesp( currFrame, detector, descriptor );
        // 比较currFrame 和 lastFrame
        RESULT_OF_PNP result = estimateMotion( lastFrame, currFrame, camera );
        if ( result.inliers < min_inliers ) //inliers不够，放弃该帧
            continue;
        // 计算运动范围是否太大
        double norm = normofTransform(result.rvec, result.tvec);
        cout<<"norm = "<<norm<<endl;
        if ( norm >= max_norm )
            continue;
        Eigen::Isometry3d T = cvMat2Eigen( result.rvec, result.tvec );
        cout<<"T="<<T.matrix()<<endl;

        // cloud = joinPointCloud( cloud, currFrame, T, camera );

        // 向g2o中增加这个顶点与上一帧联系的边
        // 顶点部分
        // 顶点只需设定id即可
        g2o::VertexSE3 *v = new g2o::VertexSE3();
        v->setId( currIndex );
        v->setEstimate( Eigen::Isometry3d::Identity() );
        globalOptimizer.addVertex(v);
        // 边部分
        g2o::EdgeSE3* edge = new g2o::EdgeSE3();
        // 连接此边的两个顶点id
        edge->vertices() [0] = globalOptimizer.vertex( lastIndex );
        edge->vertices() [1] = globalOptimizer.vertex( currIndex );
        // 信息矩阵
        Eigen::Matrix<double, 6, 6> information = Eigen::Matrix< double, 6,6 >::Identity();
        // 信息矩阵是协方差矩阵的逆，表示我们对边的精度的预先估计
        // 因为pose为6D的，信息矩阵是6*6的阵，假设位置和角度的估计精度均为0.1且互相独立
        // 那么协方差则为对角为0.01的矩阵，信息阵则为100的矩阵
        information(0,0) = information(1,1) = information(2,2) = 100;
        information(3,3) = information(4,4) = information(5,5) = 100;
        // 也可以将角度设大一些，表示对角度的估计更加准确
        edge->setInformation( information );
        // 边的估计即是pnp求解之结果
        edge->setMeasurement( T );
        // 将此边加入图中
        globalOptimizer.addEdge(edge);

        lastFrame = currFrame;
        lastIndex = currIndex;

    }

    // pcl::io::savePCDFile( "data/result.pcd", *cloud );

    // 优化所有边
    cout<<"optimizing pose graph, vertices: "<<globalOptimizer.vertices().size()<<endl;
    globalOptimizer.save("./data/result_before.g2o");
    globalOptimizer.initializeOptimization();
    globalOptimizer.optimize( 100 ); //可以指定优化步数
    globalOptimizer.save( "./data/result_after.g2o" );
    cout<<"Optimization done."<<endl;

    globalOptimizer.clear();

    return 0;
}

FRAME readFrame( int index, ParameterReader& pd )
{
    ...//省略掉了，有点长。。。
}

double normofTransform( cv::Mat rvec, cv::Mat tvec )
{
    ...//省略掉了，有点长。。。
}

整体框架也很简单，在之前的基础上增加了g2o优化，除了g2o本身需要的初始化部分；另外就是在循环计算每帧之间运动时，把每个顶点和边同时add进g2o优化器中；还有最后的整体优化部分。

第一部分：
初始化，拆解开看：
其实最核心的主干代码就这一句：

g2o::SparseOptimizer globalOptimizer;

字面意思是创建一个g2o稀疏优化器，将其命名为globalOptimizer。这一句是核心主干，其他都是辅助。

这个globalOptimizer 需要设置一下优化算法，用globalOptimizer.setAlgorithm( 这里得传进来一个优化算法 ); 方法进行设置。这个方法需要传入一个优化算法，不论是GN，LM，DL等等，反正你得是个优化算法。

OK，构造优化算法传进去就是了：g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( blockSolver ); 构造了一个LM算法，将其命名为solver (这里感觉这名字不是很贴切，可以命名为OptimizationAlgorithm)。

问题又来了，构造优化算法需要传入什么参数呢？块求解器！也就是上面的blocksolver。
构造一下吧：

g2o::BlockSolver_6_3 blockSolver = new g2o::BlockSolver_6_3(linearSolver) ;

好，这里我们又发现了一个linearSolver的东东。。。
构造块求解器需要传入线性求解器，继续构造吧：

g2o::LinearSolverCSparse< g2o::BlockSolver_6_3::PoseMatrixType >* linearSolver = new g2o::LinearSolverCSparse< g2o::BlockSolver_6_3::PoseMatrixType >();

注意就是线性稀疏求解器是模板类，需要传入顶点类型，这里就用的现成的，6*3块求解器中的位姿矩阵类型。

OK，连环套终于到了最底层，因为linearSolver的构造不再需要参数。
整个逻辑流程是：
求解器需要设置优化算法；
优化算法需要块求解器；
块求解器需要线性求解器；

写一个不是代码的东西：

optimizer.setAlgorithm( optimizationAlgorithm( blocksolver( linearsolver ) ) )

对应到代码书写流程上：
用linearsolver构造blocksolver；
用blocksolver构造optimizationAlgorithm；
用optimizationAlgorithm设置optimizer.setAlgorithm()

另外，高博的代码中为了简化，用了两个typedef：

typedef g2o::BlockSolver_6_3 SlamBlockSolver; 
typedef g2o::LinearSolverCSparse< SlamBlockSolver::PoseMatrixType > SlamLinearSolver;

分别来代替块求解器和线性求解器的类型。为了思路看起来清晰一些，所以这里将他们都带入进去了。想简化的话，可以用auto。

第二部分：
在循环中往g2o增加顶点和边：
顶点很简单，设置下ID。说是ID，其实就是一个号，第几个地点。设定下估计值，这里就是单位矩阵了，因为并没有预先估计值。最后add方法，添加进去就好了：

// 顶点部分
// 顶点只需设定id即可
g2o::VertexSE3 *v = new g2o::VertexSE3();
v->setId( currIndex );
v->setEstimate( Eigen::Isometry3d::Identity() );
globalOptimizer.addVertex(v);

边稍微复杂一些：

// 边部分
g2o::EdgeSE3* edge = new g2o::EdgeSE3();
// 连接此边的两个顶点id
edge->vertices() [0] = globalOptimizer.vertex( lastIndex );
edge->vertices() [1] = globalOptimizer.vertex( currIndex );
// 信息矩阵
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix< double, 6,6 >::Identity();
// 信息矩阵是协方差矩阵的逆，表示我们对边的精度的预先估计
// 因为pose为6D的，信息矩阵是6*6的阵，假设位置和角度的估计精度均为0.1且互相独立
// 那么协方差则为对角为0.01的矩阵，信息阵则为100的矩阵
information(0,0) = information(1,1) = information(2,2) = 100;
information(3,3) = information(4,4) = information(5,5) = 100;
// 也可以将角度设大一些，表示对角度的估计更加准确
edge->setInformation( information );
// 边的估计即是pnp求解之结果
edge->setMeasurement( T );
// 将此边加入图中
globalOptimizer.addEdge(edge);

分成三小块看：
1、设置链接的顶点，由于每个边链接两个顶点，所以是二元边，所以在edge->vertices() [0] 中只有[0] 和 [1] 这个索引指的是每条边链接的点个数，二元边就链接两个，则设置[0][1]。同理，要是一元边，只需设置[0]，三元边就是[0][1][2]这种，以此类推。
2、设置信息矩阵，注意的是信息矩阵是个6*6的矩阵，初始为单位阵，权重都一样，根据位置去调整不同权重。00 11 22为位置，33 44 55为角度，所以分开写了。设定好edge->setInformation( information ); 进行设定即可。
3、一定注意，边的估计值是pnp求解之结果：edge->setMeasurement( T );因为边是测量值，而顶点是估计值。整个优化过程是优化顶点的估计值，以此来更好的满足观测值边！所以顶点初始估计值可以没有，随着优化迭代会慢慢的接近最优值。

第四部分：
整体优化

// 优化所有边
cout<<"optimizing pose graph, vertices: "<<globalOptimizer.vertices().size()<<endl;
globalOptimizer.save("./data/result_before.g2o");
globalOptimizer.initializeOptimization();
globalOptimizer.optimize( 100 ); //可以指定优化步数
globalOptimizer.save( "./data/result_after.g2o" );
cout<<"Optimization done."<<endl;

globalOptimizer.clear();

精简掉显示输出和文件输出后，最核心的只有两句：

globalOptimizer.initializeOptimization();  //初始化
globalOptimizer.optimize( 100 );//开始迭代，设定次数为100次。

在最后的解释部分，说一个点。

顶点取成了相机姿态：g2o::VertexSE3，而边则是连接两个VertexSE3的边：g2o::EdgeSE3

可以发现顶点和边其实本质都是SE3，没有本质区别。上一张图：
这里写图片描述
这个图就是位姿图，只包含位姿X和变换T。
假如将 $X_1$ 设定为初始姿态，且位置和旋转都设定为0，此时对应的位姿矩阵是单位阵(旋转零向量通过rodrigus公式转换成旋转矩阵是单位阵！)：

X_{z e r o} = [\begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}]

$X_{zero} = \begin{bmatrix} 1 & 0 & 0 &0\\ 0 & 1 & 0 &0\\ 0 & 0 & 1 &0\\ 0 & 0 & 0 &1\\ \end{bmatrix}$

此点切记！

OK，我们的顶点： $X_1, X_2, X_3, X_4$ ；边： $T_{12}, T_{23}, T_{34}$
关系式为：

X_{2} = X_{1} T_{12} X_{3} = X_{2} T_{23} X_{4} = X_{3} T_{34}

$X_2 = X_1T_{12}\\ X_3 = X_2T_{23}\\ X_4 = X_3T_{34}$

其实将中间量全部带入后得到：

X_{2} = X_{1} T_{12} X_{3} = X_{1} T_{12} T_{23} X_{4} = X_{1} T_{12} T_{23} T_{34}

$X_2 = X_1T_{12}\\ X_3 = X_1T_{12}T_{23}\\ X_4 = X_1T_{12}T_{23}T_{34}$

又由于 $X_1$ 为单位阵，所以最终各个顶点是这样的：

X_{2} = T_{12} X_{3} = T_{12} T_{23} X_{4} = T_{12} T_{23} T_{34}

$X_2 = T_{12}\\ X_3 = T_{12}T_{23}\\ X_4 = T_{12}T_{23}T_{34}$
这里发现，其实顶点也就是一些变换(边)的组合，只不过是各个边变换累积起来的。直观的理解就是，边是相邻顶点，单一节的变换，而顶点就是将单节累积起来，当前位姿相对于初始位姿的一个总变换(这里默认了初始位姿是0状态，其实是不是0都一样。。。)。

最后说一句就是，位姿图的运算是位姿在左，变换在右，得到新的位姿：

X_{2} = X_{1} T_{12}

$X_2 = X_1T_{12}$
而在坐标系变换坐标时则是左乘：

P_{X_{2}} = T_{12} P_{X_{1}}

$P_{X_2} = T_{12} P_{X_1}$

P_{X_{1}}

$P_{X_1}$ 为在

X_{1}

$X_1$ 位姿下看到空间中P点的坐标、

P_{X_{2}}

$P_{X_2}$ 为在

X_{2}

$X_2$ 位姿下看到空间中P点的坐标。

猜你喜欢