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