[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=;//起始帧
     int endIndex=;//终止帧
     double scope=;//扫描上限距离cm
     double rejectDistance=;//m
     string path="Fubdata";
     int maxiter=;
 #pragma endregion ICPParams

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

     LoopClose loop(,);
     loopCloses->push_back(loop);
     LoopClose loop2(,);
     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 ;
     }
     int inum=-;
     vector<PoseX> poselist;
     int stIdx=-,enIdx=-;
     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)==) /*判断是否文件尾，不是则循环*/
     {
         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=;
     endIndex=poselist.size();
     int currIndex=startIndex;//当前索引
     // 向globalOptimizer增加第一个顶点
     g2o::VertexSE3* v = new g2o::VertexSE3();
     v->setId( currIndex );

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

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

         Eigen::Isometry3d p = Eigen::Isometry3d::Identity();
         p(,) = poselist[currIndex].M00;
         p(,) = poselist[currIndex].M01;
         p(,) = poselist[currIndex].M02;
         p(,) = poselist[currIndex].M03;
         p(,) = poselist[currIndex].M10;
         p(,) = poselist[currIndex].M11;
         p(,) = poselist[currIndex].M12;
         p(,) = poselist[currIndex].M13;
         p(,) = poselist[currIndex].M20;
         p(,) = poselist[currIndex].M21;
         p(,) = poselist[currIndex].M22;
         p(,) = poselist[currIndex].M23;
         p(,) = poselist[currIndex].M30;
         p(,) = poselist[currIndex].M31;
         p(,) = poselist[currIndex].M32;
         p(,) = 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() [] = globalOptimizer.vertex( currIndex-);
         edge->vertices() [] = globalOptimizer.vertex( currIndex );
         edge->setRobustKernel( robustKernel );
         // 信息矩阵
         Eigen::Matrix<double, , > information = Eigen::Matrix< double, , >::Identity();
         // 信息矩阵是协方差矩阵的逆，表示我们对边的精度的预先估计
         // 因为pose为6D的，信息矩阵是6*6的阵，假设位置和角度的估计精度均为0.1且互相独立
         // 那么协方差则为对角为0.01的矩阵，信息阵则为100的矩阵
         information(,) = information(,) = information(,) = ;
         information(,) = information(,) = information(,) = ;
         // 也可以将角度设大一些，表示对角度的估计更加准确
         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[],b[];
         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[];
         transformation= optimize0.DoICP(model,data);

         printf("-----------闭环信息---------- \n");
         printf("%f %f %f %f \n",transformation[],transformation[],transformation[],transformation[]);
         printf("%f %f %f %f \n",transformation[],transformation[],transformation[],transformation[]);
         printf("%f %f %f %f \n",transformation[],transformation[],transformation[],transformation[]);
         printf("%f %f %f %f \n",transformation[],transformation[],transformation[],transformation[]);

         g2o::RobustKernel* robustKernel = g2o::RobustKernelFactory::instance()->construct( "Cauchy" );
         // 连接此边的两个顶点id
         g2o::EdgeSE3* edge = new g2o::EdgeSE3();
         edge->vertices() [] = globalOptimizer.vertex(tmp.StartIndex);
         edge->vertices() [] = globalOptimizer.vertex(tmp.CloseIndex );
         edge->setRobustKernel( robustKernel );
         // 信息矩阵
         Eigen::Matrix<double, , > information = Eigen::Matrix< double, , >::Identity();
         // 信息矩阵是协方差矩阵的逆，表示我们对边的精度的预先估计
         // 因为pose为6D的，信息矩阵是6*6的阵，假设位置和角度的估计精度均为0.1且互相独立
         // 那么协方差则为对角为0.01的矩阵，信息阵则为100的矩阵
         information(,) = information(,) = information(,) = ;
         information(,) = information(,) = information(,) = ;
         // 也可以将角度设大一些，表示对角度的估计更加准确
         edge->setInformation( information );
         // 边的估计
         Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
         T(,) = transformation[];
         T(,) = transformation[];
         T(,) = transformation[];
         T(,) = transformation[];
         T(,) = transformation[];
         T(,) = transformation[];
         T(,) = transformation[];
         T(,) = transformation[];
         T(,) = transformation[];
         T(,) = transformation[];
         T(,) = transformation[];
         T(,) = transformation[];
         T(,) = transformation[];
         T(,) = transformation[];
         T(,) = transformation[];
         T(,) = transformation[];
         edge->setMeasurement( T);
         // 将此边加入图中
         globalOptimizer.addEdge(edge);
     }

     globalOptimizer.save("result_before.g2o");
     globalOptimizer.initializeOptimization();
     globalOptimizer.optimize(  ); //可以指定优化步数
     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[];
         sprintf(a, "%03d", i+);//位姿索引从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 ;
 }

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

 }