[g2o]一个备忘

g2o使用的一个备忘

位姿已知，闭环的帧已知，进行图优化。

#include "stdafx.h"
#include <vector>
#include "PointXYZ.h"
#include "Annicp.h"
#include <Eigen/Dense>
#include "OptimizeHelper.h"


#include "g2o/types/icp/types_icp.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/solver.h"
#include "g2o/core/robust_kernel.h"
#include <g2o/core/robust_kernel_factory.h>
#include <g2o/types/slam3d/types_slam3d.h>
#include <g2o/core/factory.h>
#include <g2o/core/optimization_algorithm_factory.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/solvers/dense/linear_solver_dense.h"
#include <g2o/solvers/csparse/linear_solver_csparse.h>

using namespace Eigen;
using namespace Eigen::internal;
using namespace Eigen::Architecture;
using namespace std;
using namespace g2o;
typedef g2o::BlockSolver_6_3 SlamBlockSolver; 
typedef g2o::LinearSolverCSparse< SlamBlockSolver::PoseMatrixType > SlamLinearSolver;

int _tmain(int argc, _TCHAR* argv[])
{
#pragma region ICPParams
    int startIndex=0;//起始帧
    int endIndex=76;//终止帧
    double scope=30000;//扫描上限距离cm
    double rejectDistance=15;//m
    string path="Fubdata";
    int maxiter=100;
#pragma endregion ICPParams

    //人工增加闭环
    vector<LoopClose> *loopCloses=new vector<LoopClose>();

    LoopClose loop(11,51);
    loopCloses->push_back(loop);
    LoopClose loop2(12,52);
    loopCloses->push_back(loop2);


    OptimizeHelper optimize0;
    // 初始化求解器
    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 );// 不要输出调试信息


    //读取已经计算出来的位姿数据
    string posefilename=path+"\\finalpose.txt";
    const char * posefile=posefilename.c_str();
    FILE* inpose=fopen(posefile,"r");
    if(inpose==NULL)
    {
        printf("missing file");
        return 0;
    }
    int inum=-1;
    vector<PoseX> poselist;
    int stIdx=-1,enIdx=-1;
    float M00,M01,M02,M03;
    float M10,M11,M12,M13;
    float M20,M21,M22,M23;
    float M30,M31,M32,M33;
    fscanf(inpose,"%d %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f",&stIdx,&enIdx,
        &M00,&M01,&M02,&M03,
        &M10,&M11,&M12,&M13,
        &M20,&M21,&M22,&M23,
        &M30,&M31,&M32,&M33); 
    while(feof(inpose)==0) /*判断是否文件尾，不是则循环*/
    {
        inum++;     
        PoseX pose(M00,M01,M02,M03,
            M10,M11,M12,M13,
            M20,M21,M22,M23,
            M30,M31,M32,M33);
        printf("%d %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",stIdx,enIdx,
            M00,M01,M02,M03,
            M10,M11,M12,M13,
            M20,M21,M22,M23,
            M30,M31,M32,M33); 
        poselist.push_back(pose);
        fscanf(inpose,"%d %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f",&stIdx,&enIdx,
            &M00,&M01,&M02,&M03,
            &M10,&M11,&M12,&M13,
            &M20,&M21,&M22,&M23,
            &M30,&M31,&M32,&M33); 
    }
    fclose(inpose);
    startIndex=0;
    endIndex=poselist.size();
    int currIndex=startIndex;//当前索引
    // 向globalOptimizer增加第一个顶点
    g2o::VertexSE3* v = new g2o::VertexSE3();
    v->setId( currIndex );

    Eigen::Isometry3d prev = Eigen::Isometry3d::Identity();
    prev(0,0) = poselist[currIndex].M00; 
    prev(0,1) = poselist[currIndex].M01; 
    prev(0,2) = poselist[currIndex].M02;
    prev(0,3) = poselist[currIndex].M03; 
    prev(1,0) = poselist[currIndex].M10; 
    prev(1,1) = poselist[currIndex].M11;
    prev(1,2) = poselist[currIndex].M12; 
    prev(1,3) = poselist[currIndex].M13; 
    prev(2,0) = poselist[currIndex].M20;
    prev(2,1) = poselist[currIndex].M21; 
    prev(2,2) = poselist[currIndex].M22; 
    prev(2,3) = poselist[currIndex].M23;
    prev(3,0) = poselist[currIndex].M30;
    prev(3,1) = poselist[currIndex].M31; 
    prev(3,2) = poselist[currIndex].M32; 
    prev(3,3) = poselist[currIndex].M33;
    v->setEstimate( prev); //估计为单位矩阵
    v->setFixed( true ); //第一个顶点固定，不用优化
    globalOptimizer.addVertex( v );
    

    for ( currIndex=startIndex+1; currIndex< endIndex; currIndex++ )
    {
        g2o::VertexSE3 *v = new g2o::VertexSE3();
        v->setId( currIndex );


        Eigen::Isometry3d p = Eigen::Isometry3d::Identity();
        p(0,0) = poselist[currIndex].M00; 
        p(0,1) = poselist[currIndex].M01; 
        p(0,2) = poselist[currIndex].M02;
        p(0,3) = poselist[currIndex].M03; 
        p(1,0) = poselist[currIndex].M10; 
        p(1,1) = poselist[currIndex].M11;
        p(1,2) = poselist[currIndex].M12; 
        p(1,3) = poselist[currIndex].M13; 
        p(2,0) = poselist[currIndex].M20;
        p(2,1) = poselist[currIndex].M21; 
        p(2,2) = poselist[currIndex].M22; 
        p(2,3) = poselist[currIndex].M23;
        p(3,0) = poselist[currIndex].M30;
        p(3,1) = poselist[currIndex].M31; 
        p(3,2) = poselist[currIndex].M32; 
        p(3,3) = poselist[currIndex].M33;

        v->setEstimate(p);
        globalOptimizer.addVertex(v);

        g2o::RobustKernel* robustKernel = g2o::RobustKernelFactory::instance()->construct( "Cauchy" );
        // 连接此边的两个顶点id
        g2o::EdgeSE3* edge = new g2o::EdgeSE3();
        edge->vertices() [0] = globalOptimizer.vertex( currIndex-1);
        edge->vertices() [1] = globalOptimizer.vertex( currIndex );
        edge->setRobustKernel( robustKernel );
        // 信息矩阵
        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 );
        // 边的估计
        Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
        T= prev.inverse() * p;
        edge->setMeasurement(T);
        // 将此边加入图中
        globalOptimizer.addEdge(edge);//注意g2o目前用的代码都是Debug版本的
        prev=p;
    }

    
    for (vector<LoopClose>::iterator iter = loopCloses->begin();iter< loopCloses->end();iter++)
    {
        LoopClose tmp=(LoopClose)*iter;
        //读取闭环的帧进行ICP匹配
        char a[10],b[10];
        sprintf(a, "%03d", tmp.StartIndex);
        sprintf(b, "%03d", tmp.CloseIndex);
        string filename=path + "\\scan" + a + ".txt";
        string filename1=path + "\\scan" + b + ".txt";
        Scan * model=new Scan();
        model->ReadScanData(filename);
        Scan * data=new Scan();
        data->ReadScanData(filename1);
        double* transformation=new double[16];
        transformation= optimize0.DoICP(model,data);
        
        printf("-----------闭环信息---------- \n");
        printf("%f %f %f %f \n",transformation[0],transformation[1],transformation[2],transformation[3]);
        printf("%f %f %f %f \n",transformation[4],transformation[5],transformation[6],transformation[7]);
        printf("%f %f %f %f \n",transformation[8],transformation[9],transformation[10],transformation[11]);
        printf("%f %f %f %f \n",transformation[12],transformation[13],transformation[14],transformation[15]);


        g2o::RobustKernel* robustKernel = g2o::RobustKernelFactory::instance()->construct( "Cauchy" );
        // 连接此边的两个顶点id
        g2o::EdgeSE3* edge = new g2o::EdgeSE3();
        edge->vertices() [0] = globalOptimizer.vertex(tmp.StartIndex);
        edge->vertices() [1] = globalOptimizer.vertex(tmp.CloseIndex );
        edge->setRobustKernel( robustKernel );
        // 信息矩阵
        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 );
        // 边的估计
        Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
        T(0,0) = transformation[0]; 
        T(0,1) = transformation[1]; 
        T(0,2) = transformation[2];
        T(0,3) = transformation[3]; 
        T(1,0) = transformation[4]; 
        T(1,1) = transformation[5];
        T(1,2) = transformation[6]; 
        T(1,3) = transformation[7]; 
        T(2,0) = transformation[8];
        T(2,1) = transformation[9]; 
        T(2,2) = transformation[10]; 
        T(2,3) = transformation[11];
        T(3,0) = transformation[12];
        T(3,1) = transformation[13]; 
        T(3,2) = transformation[14]; 
        T(3,3) = transformation[15];
        edge->setMeasurement( T);
        // 将此边加入图中
        globalOptimizer.addEdge(edge);
    }

    globalOptimizer.save("result_before.g2o");
    globalOptimizer.initializeOptimization();
    globalOptimizer.optimize( 100 ); //可以指定优化步数
    globalOptimizer.save( "result_after.g2o" );


    for (int i=startIndex; i< endIndex; i++ )
    {
        // 从g2o里取出一帧
        g2o::VertexSE3* vertex = dynamic_cast<g2o::VertexSE3*>(globalOptimizer.vertex(i ));
        Eigen::Isometry3d pose = vertex->estimate(); //该帧优化后的位姿
        char a[10];
        sprintf(a, "%03d", i+1);//位姿索引从0开始，而文件帧时从1 开始
        string filename=path + "\\scan" + a + ".txt";

        Scan * model=new Scan();
        model->ReadScanData(filename);

        Scan * newmodel=new Scan();
        model->Transform(newmodel,pose);
        string filename1=path + "\\scan" + a + "_optimal.txt";
        model->Save(filename1);
        // 把点云变换后加入全局地图中
        delete model;
        delete newmodel;
    }
    

    printf("优化完毕！\n");

    system("pause");
    return 0;
}

 

void Scan::Transform(Scan *newScan,const Eigen::Isometry3d& pose)
{
    int count=ml_pts->size();
    m_model=MatrixXd::Zero(4,count);
    int nidx=0;
    for (vector<PointXYZ>::iterator iter = ml_pts->begin();iter< ml_pts->end();iter++) //iter != modelslist->end(); ++iter)
    {
        PointXYZ tmp=(PointXYZ)*iter;
        m_model(0,nidx)=tmp.X;
        m_model(1,nidx)=tmp.Y;
        m_model(2,nidx)=tmp.Z;
        m_model(3,nidx)=1;
        nidx++;
    }
    m_model=pose.matrix()*m_model;

}