[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;

 }

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

11. 星际争霸之php设计模式--备忘模式
题记==============================================================================本php设计模式专辑来源于博客(jymo ...
PostgreSQL 速查、备忘手册 | PostgreSQL Quick Find and Tutorial
PostgreSQL 速查.备忘手册作者:汪嘉霖这是一个你可能需要的一个备忘手册,此手册方便你快速查询到你需要的常见功能.有时也有一些曾经被使用过的高级功能.如无特殊说明,此手册仅适用于 Linu ...
python序列，字典备忘
初识python备忘: 序列:列表,字符串,元组len(d),d[id],del d[id],data in d函数:cmp(x,y),len(seq),list(seq)根据字符串创建列表,max( ...
Vi命令备忘
备忘 Ctrl+u:向文件首翻半屏: Ctrl+d:向文件尾翻半屏: Ctrl+f:向文件尾翻一屏: Ctrl+b:向文件首翻一屏: Esc:从编辑模式切换到命令模式: ZZ:命令模式下保存当前文件所 ...
[备忘] Automatically reset Windows Update components
这两天遇到Windows 10的更新问题,官方有一个小工具,可以用来修复Windows Update的问题,备忘如下 https://support.microsoft.com/en-us/kb/97 ...
ECMAScript 5(ES5)中bind方法简介备忘
一直以来对和this有关的东西模糊不清,譬如call.apply等等.这次看到一个和bind有关的笔试题,故记此文以备忘. bind和call以及apply一样,都是可以改变上下文的this指向的.不 ...
MFC通过txt查找文件并进行复制-备忘
MFC基于对话框的Demo txt中每行一个23位的卡号. 文件夹中包含以卡号命名的图像文件.(fpt或者bmp文件) 要求遍历文件夹,找到txt中卡号所对应的图像文件,并复制出来. VC6.0写的. ...
Javascript备忘模式
使用备忘模式,利用了函数的自定义属性,先看一个例子 var test = function (){} test.myAttr = "attr"; 这样,就给test加上了一个自定义 ...
Cheat (tldr, bropages) - Unix命令用法备忘单
cheat 是一个Unix命令行小工具,用来查询一些常用命令的惯用法(我们都知道,man page阅读起来太累了,常常是跳到最后去看 examples,但并不是所有man pages里面都有examp ...

随机推荐

NBUT 1457 Sona（莫队算法+离散化）
[1457] Sona 时间限制: 5000 ms 内存限制: 65535 K 问题描述 Sona, Maven of the Strings. Of cause, she can play the ...
暑假训练round 3 题解
今天做题运气出奇的好,除了几处小错误调试之后忘记改掉了……最后还AK了……虽然题目不难,学长也说是福利局,但是对个人的鼓励作用还是挺大的……至此暑假训练就结束了,也算没有遗憾……. 题解如下: Pro ...
Ubuntu 14.04 安装 VirtualBox
参考: ubuntu14.04,安装VirtualBox 5.0(虚拟机软件)! 由于Vagrant工具的需要,安装了一下VirtualBox. 使用参考链接的法一居然在软件中心里面报错,我想可能是没 ...
php字符串常用函数
addslashes print addslahes ('She said, "Great!"'); #output #She said, \"Great!\ echo ...
HTML: 用表格畫出課程表
這個知識點,常常被我忽略,而且誤解(其實不是我誤解),曾經一個公司的要求:不使用table,一律用div,即使是整齊的,能夠使用table輕鬆佈局出的樣式,也一定要用div. 可能這傢伙沒搞清楚,ta ...
sqlserver log
DBCC LOGINFODBCC log('QSSys', TYPE=2)goselect * from sys.fn_dblog(null,null)select [Dirty Pages],[Mi ...
区分super和this
Java关键字this.super使用总结一.this Java关键字this只能用于方法方法体内.当一个对象创建后,Java虚拟机(JVM)就会给这个对象分配一个引用自身的指针,这个指针的名字就是 ...
laravel paginate动态分页
1.router Route::get('product', function(){ $products = App\Product::paginate(10); return view('produ ...
ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/
https://en.wikipedia.org/wiki/Base64 The Base64 index table: Value Char Value Char Value Char ...
android 设计工具栏
设计工具栏Action Bar(订制工具栏类型) 工具栏给用户提供了一种熟悉和可预测的方式来执行某种动作和操纵应用程序,但是这并不意味着它就需要和其他的应用程序看起来一样的.如果想设计工具栏以使得它能 ...

[g2o]一个备忘

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

随机推荐

热门专题