g2o使用的一个备忘
位姿已知，闭环的帧已知，进行图优化。
1 #include "stdafx.h" 2 #include <vector> 3 #include "PointXYZ.h" 4 #include "Annicp.h" 5 #include <Eigen/Dense> 6 #include "OptimizeHelper.h" 7 8 9 #include "g2o/types/icp/types_icp.h" 10 #include "g2o/core/sparse_optimizer.h" 11 #include "g2o/core/block_solver.h" 12 #include "g2o/core/solver.h" 13 #include "g2o/core/robust_kernel.h" 14 #include <g2o/core/robust_kernel_factory.h> 15 #include <g2o/types/slam3d/types_slam3d.h> 16 #include <g2o/core/factory.h> 17 #include <g2o/core/optimization_algorithm_factory.h> 18 #include <g2o/core/optimization_algorithm_gauss_newton.h> 19 #include "g2o/core/optimization_algorithm_levenberg.h" 20 #include "g2o/solvers/dense/linear_solver_dense.h" 21 #include <g2o/solvers/csparse/linear_solver_csparse.h> 22 23 using namespace Eigen; 24 using namespace Eigen::internal; 25 using namespace Eigen::Architecture; 26 using namespace std; 27 using namespace g2o; 28 typedef g2o::BlockSolver_6_3 SlamBlockSolver; 29 typedef g2o::LinearSolverCSparse< SlamBlockSolver::PoseMatrixType > SlamLinearSolver; 30 31 int _tmain(int argc, _TCHAR* argv[]) 32 { 33 #pragma region ICPParams 34 int startIndex=0;//起始帧 35 int endIndex=76;//终止帧 36 double scope=30000;//扫描上限距离cm 37 double rejectDistance=15;//m 38 string path="Fubdata"; 39 int maxiter=100; 40 #pragma endregion ICPParams 41 42 //人工增加闭环 43 vector<LoopClose> *loopCloses=new vector<LoopClose>(); 44 45 LoopClose loop(11,51); 46 loopCloses->push_back(loop); 47 LoopClose loop2(12,52); 48 loopCloses->push_back(loop2); 49 50 51 OptimizeHelper optimize0; 52 // 初始化求解器 53 SlamLinearSolver* linearSolver = new SlamLinearSolver(); 54 linearSolver->setBlockOrdering( false ); 55 SlamBlockSolver* blockSolver = new SlamBlockSolver( linearSolver ); 56 g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( blockSolver ); 57 58 g2o::SparseOptimizer globalOptimizer; // 最后用的就是这个东东 59 globalOptimizer.setAlgorithm( solver ); 60 globalOptimizer.setVerbose( false );// 不要输出调试信息 61 62 63 //读取已经计算出来的位姿数据 64 string posefilename=path+"\finalpose.txt"; 65 const char * posefile=posefilename.c_str(); 66 FILE* inpose=fopen(posefile,"r"); 67 if(inpose==NULL) 68 { 69 printf("missing file"); 70 return 0; 71 } 72 int inum=-1; 73 vector<PoseX> poselist; 74 int stIdx=-1,enIdx=-1; 75 float M00,M01,M02,M03; 76 float M10,M11,M12,M13; 77 float M20,M21,M22,M23; 78 float M30,M31,M32,M33; 79 fscanf(inpose,"%d %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f",&stIdx,&enIdx, 80 &M00,&M01,&M02,&M03, 81 &M10,&M11,&M12,&M13, 82 &M20,&M21,&M22,&M23, 83 &M30,&M31,&M32,&M33); 84 while(feof(inpose)==0) /*判断是否文件尾，不是则循环*/ 85 { 86 inum++; 87 PoseX pose(M00,M01,M02,M03, 88 M10,M11,M12,M13, 89 M20,M21,M22,M23, 90 M30,M31,M32,M33); 91 printf("%d %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f ",stIdx,enIdx, 92 M00,M01,M02,M03, 93 M10,M11,M12,M13, 94 M20,M21,M22,M23, 95 M30,M31,M32,M33); 96 poselist.push_back(pose); 97 fscanf(inpose,"%d %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f",&stIdx,&enIdx, 98 &M00,&M01,&M02,&M03, 99 &M10,&M11,&M12,&M13, 100 &M20,&M21,&M22,&M23, 101 &M30,&M31,&M32,&M33); 102 } 103 fclose(inpose); 104 startIndex=0; 105 endIndex=poselist.size(); 106 int currIndex=startIndex;//当前索引 107 // 向globalOptimizer增加第一个顶点 108 g2o::VertexSE3* v = new g2o::VertexSE3(); 109 v->setId( currIndex ); 110 111 Eigen::Isometry3d prev = Eigen::Isometry3d::Identity(); 112 prev(0,0) = poselist[currIndex].M00; 113 prev(0,1) = poselist[currIndex].M01; 114 prev(0,2) = poselist[currIndex].M02; 115 prev(0,3) = poselist[currIndex].M03; 116 prev(1,0) = poselist[currIndex].M10; 117 prev(1,1) = poselist[currIndex].M11; 118 prev(1,2) = poselist[currIndex].M12; 119 prev(1,3) = poselist[currIndex].M13; 120 prev(2,0) = poselist[currIndex].M20; 121 prev(2,1) = poselist[currIndex].M21; 122 prev(2,2) = poselist[currIndex].M22; 123 prev(2,3) = poselist[currIndex].M23; 124 prev(3,0) = poselist[currIndex].M30; 125 prev(3,1) = poselist[currIndex].M31; 126 prev(3,2) = poselist[currIndex].M32; 127 prev(3,3) = poselist[currIndex].M33; 128 v->setEstimate( prev); //估计为单位矩阵 129 v->setFixed( true ); //第一个顶点固定，不用优化 130 globalOptimizer.addVertex( v ); 131 132 133 for ( currIndex=startIndex+1; currIndex< endIndex; currIndex++ ) 134 { 135 g2o::VertexSE3 *v = new g2o::VertexSE3(); 136 v->setId( currIndex ); 137 138 139 Eigen::Isometry3d p = Eigen::Isometry3d::Identity(); 140 p(0,0) = poselist[currIndex].M00; 141 p(0,1) = poselist[currIndex].M01; 142 p(0,2) = poselist[currIndex].M02; 143 p(0,3) = poselist[currIndex].M03; 144 p(1,0) = poselist[currIndex].M10; 145 p(1,1) = poselist[currIndex].M11; 146 p(1,2) = poselist[currIndex].M12; 147 p(1,3) = poselist[currIndex].M13; 148 p(2,0) = poselist[currIndex].M20; 149 p(2,1) = poselist[currIndex].M21; 150 p(2,2) = poselist[currIndex].M22; 151 p(2,3) = poselist[currIndex].M23; 152 p(3,0) = poselist[currIndex].M30; 153 p(3,1) = poselist[currIndex].M31; 154 p(3,2) = poselist[currIndex].M32; 155 p(3,3) = poselist[currIndex].M33; 156 157 v->setEstimate(p); 158 globalOptimizer.addVertex(v); 159 160 g2o::RobustKernel* robustKernel = g2o::RobustKernelFactory::instance()->construct( "Cauchy" ); 161 // 连接此边的两个顶点id 162 g2o::EdgeSE3* edge = new g2o::EdgeSE3(); 163 edge->vertices() [0] = globalOptimizer.vertex( currIndex-1); 164 edge->vertices() [1] = globalOptimizer.vertex( currIndex ); 165 edge->setRobustKernel( robustKernel ); 166 // 信息矩阵 167 Eigen::Matrix<double, 6, 6> information = Eigen::Matrix< double, 6,6 >::Identity(); 168 // 信息矩阵是协方差矩阵的逆，表示我们对边的精度的预先估计 169 // 因为pose为6D的，信息矩阵是6*6的阵，假设位置和角度的估计精度均为0.1且互相独立 170 // 那么协方差则为对角为0.01的矩阵，信息阵则为100的矩阵 171 information(0,0) = information(1,1) = information(2,2) = 100; 172 information(3,3) = information(4,4) = information(5,5) = 100; 173 // 也可以将角度设大一些，表示对角度的估计更加准确 174 edge->setInformation( information ); 175 // 边的估计 176 Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); 177 T= prev.inverse() * p; 178 edge->setMeasurement(T); 179 // 将此边加入图中 180 globalOptimizer.addEdge(edge);//注意g2o目前用的代码都是Debug版本的 181 prev=p; 182 } 183 184 185 for (vector<LoopClose>::iterator iter = loopCloses->begin();iter< loopCloses->end();iter++) 186 { 187 LoopClose tmp=(LoopClose)*iter; 188 //读取闭环的帧进行ICP匹配 189 char a[10],b[10]; 190 sprintf(a, "%03d", tmp.StartIndex); 191 sprintf(b, "%03d", tmp.CloseIndex); 192 string filename=path + "\scan" + a + ".txt"; 193 string filename1=path + "\scan" + b + ".txt"; 194 Scan * model=new Scan(); 195 model->ReadScanData(filename); 196 Scan * data=new Scan(); 197 data->ReadScanData(filename1); 198 double* transformation=new double[16]; 199 transformation= optimize0.DoICP(model,data); 200 201 printf("-----------闭环信息---------- "); 202 printf("%f %f %f %f ",transformation[0],transformation[1],transformation[2],transformation[3]); 203 printf("%f %f %f %f ",transformation[4],transformation[5],transformation[6],transformation[7]); 204 printf("%f %f %f %f ",transformation[8],transformation[9],transformation[10],transformation[11]); 205 printf("%f %f %f %f ",transformation[12],transformation[13],transformation[14],transformation[15]); 206 207 208 g2o::RobustKernel* robustKernel = g2o::RobustKernelFactory::instance()->construct( "Cauchy" ); 209 // 连接此边的两个顶点id 210 g2o::EdgeSE3* edge = new g2o::EdgeSE3(); 211 edge->vertices() [0] = globalOptimizer.vertex(tmp.StartIndex); 212 edge->vertices() [1] = globalOptimizer.vertex(tmp.CloseIndex ); 213 edge->setRobustKernel( robustKernel ); 214 // 信息矩阵 215 Eigen::Matrix<double, 6, 6> information = Eigen::Matrix< double, 6,6 >::Identity(); 216 // 信息矩阵是协方差矩阵的逆，表示我们对边的精度的预先估计 217 // 因为pose为6D的，信息矩阵是6*6的阵，假设位置和角度的估计精度均为0.1且互相独立 218 // 那么协方差则为对角为0.01的矩阵，信息阵则为100的矩阵 219 information(0,0) = information(1,1) = information(2,2) = 100; 220 information(3,3) = information(4,4) = information(5,5) = 100; 221 // 也可以将角度设大一些，表示对角度的估计更加准确 222 edge->setInformation( information ); 223 // 边的估计 224 Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); 225 T(0,0) = transformation[0]; 226 T(0,1) = transformation[1]; 227 T(0,2) = transformation[2]; 228 T(0,3) = transformation[3]; 229 T(1,0) = transformation[4]; 230 T(1,1) = transformation[5]; 231 T(1,2) = transformation[6]; 232 T(1,3) = transformation[7]; 233 T(2,0) = transformation[8]; 234 T(2,1) = transformation[9]; 235 T(2,2) = transformation[10]; 236 T(2,3) = transformation[11]; 237 T(3,0) = transformation[12]; 238 T(3,1) = transformation[13]; 239 T(3,2) = transformation[14]; 240 T(3,3) = transformation[15]; 241 edge->setMeasurement( T); 242 // 将此边加入图中 243 globalOptimizer.addEdge(edge); 244 } 245 246 globalOptimizer.save("result_before.g2o"); 247 globalOptimizer.initializeOptimization(); 248 globalOptimizer.optimize( 100 ); //可以指定优化步数 249 globalOptimizer.save( "result_after.g2o" ); 250 251 252 for (int i=startIndex; i< endIndex; i++ ) 253 { 254 // 从g2o里取出一帧 255 g2o::VertexSE3* vertex = dynamic_cast<g2o::VertexSE3*>(globalOptimizer.vertex(i )); 256 Eigen::Isometry3d pose = vertex->estimate(); //该帧优化后的位姿 257 char a[10]; 258 sprintf(a, "%03d", i+1);//位姿索引从0开始，而文件帧时从1 开始 259 string filename=path + "\scan" + a + ".txt"; 260 261 Scan * model=new Scan(); 262 model->ReadScanData(filename); 263 264 Scan * newmodel=new Scan(); 265 model->Transform(newmodel,pose); 266 string filename1=path + "\scan" + a + "_optimal.txt"; 267 model->Save(filename1); 268 // 把点云变换后加入全局地图中 269 delete model; 270 delete newmodel; 271 } 272 273 274 printf("优化完毕！ "); 275 276 system("pause"); 277 return 0; 278 }
1 void Scan::Transform(Scan *newScan,const Eigen::Isometry3d& pose) 2 { 3 int count=ml_pts->size(); 4 m_model=MatrixXd::Zero(4,count); 5 int nidx=0; 6 for (vector<PointXYZ>::iterator iter = ml_pts->begin();iter< ml_pts->end();iter++) //iter != modelslist->end(); ++iter) 7 { 8 PointXYZ tmp=(PointXYZ)*iter; 9 m_model(0,nidx)=tmp.X; 10 m_model(1,nidx)=tmp.Y; 11 m_model(2,nidx)=tmp.Z; 12 m_model(3,nidx)=1; 13 nidx++; 14 } 15 m_model=pose.matrix()*m_model; 16 17 }