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;

 }

[g2o]一个备忘的更多相关文章

  1. 11. 星际争霸之php设计模式--备忘模式

    题记==============================================================================本php设计模式专辑来源于博客(jymo ...

  2. PostgreSQL 速查、备忘手册 | PostgreSQL Quick Find and Tutorial

    PostgreSQL 速查.备忘手册 作者:汪嘉霖 这是一个你可能需要的一个备忘手册,此手册方便你快速查询到你需要的常见功能.有时也有一些曾经被使用过的高级功能.如无特殊说明,此手册仅适用于 Linu ...

  3. python序列，字典备忘

    初识python备忘: 序列:列表,字符串,元组len(d),d[id],del d[id],data in d函数:cmp(x,y),len(seq),list(seq)根据字符串创建列表,max( ...

  4. Vi命令备忘

    备忘 Ctrl+u:向文件首翻半屏: Ctrl+d:向文件尾翻半屏: Ctrl+f:向文件尾翻一屏: Ctrl+b:向文件首翻一屏: Esc:从编辑模式切换到命令模式: ZZ:命令模式下保存当前文件所 ...

  5. [备忘] Automatically reset Windows Update components

    这两天遇到Windows 10的更新问题,官方有一个小工具,可以用来修复Windows Update的问题,备忘如下 https://support.microsoft.com/en-us/kb/97 ...

  6. ECMAScript 5(ES5)中bind方法简介备忘

    一直以来对和this有关的东西模糊不清,譬如call.apply等等.这次看到一个和bind有关的笔试题,故记此文以备忘. bind和call以及apply一样,都是可以改变上下文的this指向的.不 ...

  7. MFC通过txt查找文件并进行复制-备忘

    MFC基于对话框的Demo txt中每行一个23位的卡号. 文件夹中包含以卡号命名的图像文件.(fpt或者bmp文件) 要求遍历文件夹,找到txt中卡号所对应的图像文件,并复制出来. VC6.0写的. ...

  8. Javascript备忘模式

    使用备忘模式,利用了函数的自定义属性,先看一个例子 var test = function (){} test.myAttr = "attr"; 这样,就给test加上了一个自定义 ...

  9. Cheat (tldr, bropages) - Unix命令用法备忘单

    cheat 是一个Unix命令行小工具,用来查询一些常用命令的惯用法(我们都知道,man page阅读起来太累了,常常是跳到最后去看 examples,但并不是所有man pages里面都有examp ...

随机推荐

  1. 【新产品发布】【iM_TFTRGB 液晶驱动模块】

    ============================== 技术论坛:http://www.eeschool.org 博客地址:http://xiaomagee.cnblogs.com 官方网店:h ...

  2. PHP下利用PHPMailer配合QQ邮箱下的域名邮箱发送邮件（转）

    首先确定不是开启socks openssl phpinfo就可以知道 下载phpmailer   地址:https://github.com/PHPMailer/PHPMailer 下载完整, 个人和 ...

  3. Jenkins学习记录

    参考资料 官方文档 用MSBuild和Jenkins搭建持续集成环境(1) 用MSBuild和Jenkins搭建持续集成环境(2) 构建基于Jenkins + Github的持续集成环境 Jenkin ...

  4. twitter storm源码走读之7 -- trident topology可靠性分析

    欢迎转载,转载请注明出处,徽沪一郎. 本文详细分析TridentTopology的可靠性实现, TridentTopology通过transactional spout与transactional s ...

  5. 将Asset中的数据库文件拷贝出来使用

    设置保存路径 private final static String DATABASE_PATH = "/data"+ Environment.getDataDirectory() ...

  6. 自动更新开奖数据的excel文件，供大家下载

    自动更新开奖数据的excel文件,供大家下载 2010-03-14 20:22 228492人阅读打印来源:乐彩网 作者:eren 很多人拥有自制excel电子表格,常要更新最基本的开奖信息.如有多期 ...

  7. Xamarin Visual Studio无法debug

    在Visual Studio中,Target IOS Device下拉框是禁用状态,无法选择. Xamarin论坛中有不少关于这个问题的,如下面这个帖子: http://forums.xamarin. ...

  8. linux下利用curl监控网页shell脚本

    #!/bin/bash smail() {mail -s "$1" gjw_apparitor@gmail.com <<EOF$1$2====report time: ...

  9. 关于C语言读取多行数据的问题

    我有如下数据格式3360  2001     5   1750.5   1246.5  22.5  n    1775=1177-1316:13360  2001     5   1750.5   1 ...

  10. 【转】Unity 解析Json字符串

    http://blog.csdn.net/qq_15267341/article/details/52013190 LitJSON使用很简单,两个步骤: 1 将LitJSON.dll文件拖动到unit ...