非线性优化库g2o实战中,基本按照如上图5步进行;
02
G2O实战套路
2.0准备工作
BlockSolver
1.BlockSolver用于计算稀疏的雅可比矩阵和Hessian矩阵;
2.BlockSolverTraits
:P表示姿态纬度(流形manifold下最小表示),L表示三维点维度;为构建雅可比矩阵和Hessian矩阵提供尺度;
3.为便于使用,此处采用typedef缩写,或系统自带缩写:
typedefg2o::BlockSolver
typedefBlockSolver
//用于BA/3Dslam;
typedefBlockSolver
//用于带尺度的BA/3Dslam;
typedefBlockSolver
//用于BA/2D3自由度slam;
此外还具有变尺寸:
usingBlockSolverX=BlockSolverPL Eigen::Dynamic>; 2.1建立线性求解器 建立线性求解器LinearSolver,用于H△x=b,△x=H.inv()*b; (结合实际情况替换Block) 第1步 Block::LinearSolverType*linearSolver=new g2o::LinearSolverDense(Block::PoseMatrixType); 其他求解器 LinearSolverDense 使用denseCholesky分解法 LinearSolverEigen 依赖项只有eigen,与Csparse差不多 LinearSolverCholmod 使用sparsecholesky分解法 LinearSolverCSparse LinearSolverPCG 使用preconditionedconjugategradient 2.2建立块求解器 建立块求解器BlockSolver,用LinearSolver对其初始化; 第2步 Block*solver_ptr=new Block(std::unique_ptr (linearSolver)); 2.3建立总求解器 从GN/LM/DogLeg中选择优化算法,并用块求解器初始化 第3步 g2o::OptimizationAlgorithmLevenberg*solver=new g2o:OptimizationAlgorithmLevenberg(std::unique_ptr 其他优化算法求解器 OptimizationAlgorithmGaussNewton 高斯牛顿 OptimizationAlgorithmDogleg DL 2.4建立稀疏优化器 建立稀疏优化器optimizer,并为其设定求解器 第4步 g2o::SparseOptimizeroptimizer; optimizer.setAlgorithm(solver); optimizer.setVerbose(true);//打开调试输出 2.5添加顶点 第5步 5.1初始化顶点 g2o::VertexSE3Expmap*vSE3= newg2o::VertexSE3Expmap(); 5.2顶点赋初值 vSE3->setEstimate( Converter::soSE3Quat(pFrame->mTcw));//setEstimate是继承而来 5.3顶点设定编号 vSE3->setId(idx); //如果单个节点就是0; //多个节点看情况确定,setId是继承而来; 5.4顶点是否参与优化 vSE3->setFixed(false); //参与优化所以无需固定; //有些定点只是提供约束不优化所以需要固定true 5.5顶点是否对齐shur,针对三维点类型 vSE3->setMarginalized(false); 5.6顶点加入优化器optimizer optimizer.addVertex(vSE3); //addVertex是继承而来 自定义顶点 classVertexSE3Expmap::public g2o::BaseVertex<6,SE3Quat>//6:流形空间中定点的最小维度表示 //SE3Quat:顶点数据类型 { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW//Eigen库在类当中使用时必须添加,以便位对齐 VertexSE3Expmap(){}//顶点构造函数 virtualvoidread(std::istream&is){} virtualvoidwrite(std::ostream&os)const{} //read和write函数一般情况下可忽略 virtualvoidsetOriginImpl()//设定顶点初值或重置 _estimate=SE3Quat(); //SE3Quat是顶点中成员的初始化方式 } //例如构造函数 SE3Quat(){ _r.setIdentity(); _t.setZero();} virtualvoidoplusImpl(constdouble*update)override//定点的更新方式 //Eigen::Map _estimate+=/*update*/; //setEstimate(SE3Quat::exp(update)*estimate()); 常见顶点 VertexSE2:publicBaseVertex<3,SE2> //2DposeVertex,(x,y,theta) VertexSE3:publicBaseVertex<6,Isometry3> //6dvector(x,y,z,qx,qy,qz)(notethatwe leaveoutthewpartofthequaternion) VertexPointXY:publicBaseVertex<2,Vector2> VertexPointXYZ:publicBaseVertex<3,Vector3> VertexSBAPointXYZ:publicBaseVertex<3,Vector3> //SE3Vertexparameterizedinternallywithatransformationmatrixandexternallywith itsexponentialmap VertexSE3Expmap:publicBaseVertex<6,SE3Quat> //SBACamVertex,(x,y,z,qw,qx,qy,qz),(x,y,z,qx,qy,qz)(notethatweleaveoutthewpart ofthequaternion. //qwisassumedtobepositive,otherwisethereisanambiguityinqx,qy,qzasa rotation VertexCam:publicBaseVertex<6,SBACam> //Sim3Vertex,(x,y,z,qw,qx,qy,qz),7dvector,(x,y,z,qx,qy,qz)(notethatweleaveout thewpartofthequaternion. VertexSim3Expmap:publicBaseVertex<7,Sim3> 2.6添加边 第6步 6.1初始化边 g2o::EdgeSE3ProjectXYZOnlyPose*e=newg2o::EdgeSE3ProjectXYZOnlyPose(); 6.2设置边的顶点 e->setVertex(0, dynamics_cast //单元边 e->setVertex(1,pose); //如果是双元边,则0对应三维点顶点, //1对应位姿顶点 6.3设置id e->setId(i); 6.4设置测量值 e->setMeasurement(obs); //obs是测量值 6.5设置协方差矩阵 e->setInformation( Eigen::Matrix2d::Identity()*inSigma2); 6.6设置鲁邦函数 g2o::RobustKernelHuber*rk= newg2o::RobustKernelHuber; e->setRobustKernel(rk); e->fx=pFrame->fx; e->fy=pFrame->fy; e->cx=pFrame->cx; e->cy=pFrame->cy; 6.8添加边 optimizer.addEdge(e); 自定义-单元边 classEdgeSE3ProjectXYZOnlyPose:publicg2o:: BaseUnaryEdge<2,vector2d,VertexSE3Expmap> /单元边<测量值维度,测量值类型,边对应的定点> EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgeSE3ProjectXYZOnlyPose(){} virtualboolread(istream&in){} virtualboolwrite(ostream&out)const{} virtualvoidcomputeError()override #计算测量值和顶点计算值的误差 constVertexSE3Expmap*v1=static_cast vector2dobs(_measurement); _error=obs–cam_project(v1->estimate().map(Xw)); //利用定点v1的变换矩阵使用map将世界坐标转换到 //相机坐标,然后cam_project投影到图像坐标 virtualvoidlinearizeOplus()override //当前顶点值下该误差对优化变量的偏导数, //雅可比矩阵 private: Vector3dXw;doublefx,fy,cx,cy; Vector2CameraParameters::cam_project(constVector3&trans_xyz)const{ Vector2proj=project2d(trans_xyz); Vector2res; res[0]=proj[0]*fx+cx; res[1]=proj[1]*fy+cy; returnres; Vector3map(constVector3&xyz)const{ returns*(r*xyz)+t; 自定义-双元边 classEdgeProjectXYZ2UV:publicBaseBinaryEdge <2,Vector2D, VertexSBAPointXYZ,VertexSE3Expmap> /双元边<测量值维度,测量值类型,边对应的定点> EIGEN_MAKE_ALIGNED_OPERATOR_NEW; //1.默认初始化 EdgeProjectXYZ2UV(); //2.计算误差 voidcomputeError() //李群相机位姿v1 constVertexSE3Expmap*v1=static_cast //顶点v2 constVertexSBAPointXYZ*v2=static_cast //相机参数 constCameraParameters*cam =static_cast //误差计算,测量值减去估计值,也就是重投影误差obs-cam //估计值计算方法是T*p,得到相机坐标系下坐标,然后在利用camera2pixel()函数得到像素坐标。 Vector2Dobs(_measurement); _error=obs-cam->cam_map (v1->estimate().map(v2->estimate())); //3.线性增量函数,也就是雅克比矩阵J的计算方法 virtualvoidlinearizeOplus(); //4.相机参数 CameraParameters*_cam; boolread(std::istream&is); boolwrite(std::ostream&os)const; }; G2O重要成员变量和函数 _measurement:存储观测值 _error:存储computeError()函数计算的误差 _vertices[]:存储顶点信息,比如二元边的话,_vertices[]的大小为2,存储顺序和调用setVertex(int,vertex)是设定的int有关(0或1)