01 G2O框架预览 非线性优化库g2o实战中，基本按照如上图5步进行； 02 G2O实战套路 2.0 准备工作 BlockSolver 1.BlockSolver用于计算稀疏的雅可比矩阵和Hessian矩阵； 2.BlockSolverTraits<P, L>:P表示姿态纬度(流形manifold下最小表示)，L表示三维点维度；为构建雅可比矩阵和Hessian矩阵提供尺度； 3.为便于使用，此处采用typedef缩写，或系统自带缩写: typedef g2o::BlockSolver<BlockSolverTraits<3,1>> Block； typedef BlockSolver<BlockSolverTraits<6,3>> BlockSolver_6_3; //用于BA/3D slam； typedef BlockSolver<BlockSolverTraits<7,3>> BlockSolver_7_3; //用于带尺度的BA/3D slam； typedef BlockSolver<BlockSolverTraits<3,2>> BlockSolver_3_2; //用于BA/2D 3自由度 slam； 此外还具有变尺寸： using BlockSolverX = BlockSolverPL<Eigen::Dynamic, Eigen::Dynamic>; 2.1 建立线性求解器 建立线性求解器LinearSolver，用于H△x=b，△x=H.inv()*b; (结合实际情况替换Block) 第1步 Block::LinearSolverType* linearSolver =new g2o::LinearSolverDense(Block::PoseMatrixType); 其他求解器 LinearSolverDense 使用dense Cholesky分解法 LinearSolverEigen 依赖项只有eigen，与Csparse差不多 LinearSolverCholmod 使用sparse cholesky分解法 LinearSolverCSparse LinearSolverPCG 使用preconditioned conjugate gradient 2.2 建立块求解器 建立块求解器BlockSolver，用LinearSolver对其初始化； 第2步 Block* solver_ptr = new Block(std::unique_ptr<Block::LinearSolverType> (linearSolver)); 2.3 建立总求解器 从GN/LM/DogLeg中选择优化算法，并用块求解器初始化 第3步 g2o::OptimizationAlgorithmLevenberg* solver =new g2o:OptimizationAlgorithmLevenberg(std::unique_ptr<Block> (solver_ptr)); 其他优化算法求解器 OptimizationAlgorithmGaussNewton 高斯牛顿 OptimizationAlgorithmDogleg DL 2.4 建立稀疏优化器 建立稀疏优化器optimizer，并为其设定求解器 第4步 g2o::SparseOptimizer optimizer; optimizer.setAlgorithm(solver); optimizer.setVerbose(true); //打开调试输出 2.5 添加顶点 第5步 5.1 初始化顶点 g2o::VertexSE3Expmap* vSE3 = new g2o::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是继承而来 自定义顶点 class VertexSE3Expmap::public g2o::BaseVertex<6, SE3Quat> //6：流形空间中定点的最小维度表示 //SE3Quat:顶点数据类型 { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW //Eigen库在类当中使用时必须添加，以便位对齐 VertexSE3Expmap (){} //顶点构造函数 virtual void read(std::istream& is) {} virtual void write(std::ostream& os) const {} //read和write函数一般情况下可忽略 virtual void setOriginImpl() //设定顶点初值或重置 { _estimate = SE3Quat (); //SE3Quat是顶点中成员的初始化方式 } //例如构造函数 SE3Quat(){ _r.setIdentity(); _t.setZero(); } virtual void oplusImpl(const double* update) override //定点的更新方式 { //Eigen::Map<const Vector6d>update(_update); _estimate += /*update*/; //setEstimate(SE3Quat::exp(update)*estimate()); } } 常见顶点 VertexSE2 : public BaseVertex<3, SE2> //2D pose Vertex, (x,y,theta) VertexSE3 : public BaseVertex<6, Isometry3> //6d vector (x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion) VertexPointXY : public BaseVertex<2, Vector2> VertexPointXYZ : public BaseVertex<3, Vector3> VertexSBAPointXYZ : public BaseVertex<3, Vector3> // SE3 Vertex parameterized internally with a transformation matrix and externally with its exponential map VertexSE3Expmap : public BaseVertex<6, SE3Quat> // SBACam Vertex, (x,y,z,qw,qx,qy,qz),(x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion. // qw is assumed to be positive, otherwise there is an ambiguity in qx,qy,qz as a rotation VertexCam : public BaseVertex<6, SBACam> // Sim3 Vertex, (x,y,z,qw,qx,qy,qz),7d vector,(x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion. VertexSim3Expmap : public BaseVertex<7, Sim3> 2.6 添加边 第6步 6.1 初始化边 g2o::EdgeSE3ProjectXYZOnlyPose* e = new g2o:: EdgeSE3ProjectXYZOnlyPose (); 6.2 设置边的顶点 e->setVertex(0, dynamics_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0))); //单元边 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 = new g2o:: RobustKernelHuber; e->setRobustKernel(rk); 6.7 设置边中其他相关参数，若有 e->fx = pFrame->fx; e->fy = pFrame->fy; e->cx = pFrame->cx; e->cy = pFrame->cy; 6.8 添加边 optimizer.addEdge(e); 自定义-单元边 class EdgeSE3ProjectXYZOnlyPose: public g2o:: BaseUnaryEdge<2,vector2d, VertexSE3Expmap> /单元边<测量值维度,测量值类型,边对应的定点> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgeSE3ProjectXYZOnlyPose (){} virtual bool read(istream& in) {} virtual bool write(ostream& out) const {} virtual void computeError() override #计算测量值和顶点计算值的误差 { const VertexSE3Expmap* v1 = static_cast <const VertexSE3Expmap*>(_vertices[0]); vector2d obs(_measurement); _error=obs–cam_project(v1->estimate().map(Xw)); //利用定点v1的变换矩阵使用map将世界坐标转换到 //相机坐标，然后cam_project投影到图像坐标 } virtual void linearizeOplus() override //当前顶点值下该误差对优化变量的偏导数， //雅可比矩阵 { } private: Vector3d Xw; double fx,fy,cx,cy; } Vector2 CameraParameters::cam_project(const Vector3 & trans_xyz) const { Vector2 proj = project2d(trans_xyz); Vector2 res; res[0] = proj[0]*fx+ cx; res[1] = proj[1]*fy + cy; return res; Vector3 map (const Vector3& xyz) const { return s*(r*xyz) + t; } 自定义-双元边 class EdgeProjectXYZ2UV : public BaseBinaryEdge <2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap> /双元边<测量值维度，测量值类型，边对应的定点> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; //1. 默认初始化 EdgeProjectXYZ2UV(); //2. 计算误差 void computeError() { //李群相机位姿v1 const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]); // 顶点v2 const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]); //相机参数 const CameraParameters * cam = static_cast<const CameraParameters *>(parameter(0)); //误差计算，测量值减去估计值，也就是重投影误差obs-cam //估计值计算方法是T*p,得到相机坐标系下坐标，然后在利用camera2pixel()函数得到像素坐标。 Vector2D obs(_measurement); _error = obs-cam->cam_map (v1->estimate().map(v2->estimate())); } //3. 线性增量函数，也就是雅克比矩阵J的计算方法 virtual void linearizeOplus(); //4. 相机参数 CameraParameters * _cam; bool read(std::istream& is); bool write(std::ostream& os) const; }; G2O重要成员变量和函数 _measurement：存储观测值 _error：存储computeError() 函数计算的误差 _vertices[]：存储顶点信息，比如二元边的话，_vertices[] 的大小为2，存储顺序和调用setVertex(int, vertex) 是设定的int 有关（0 或1） setId(int)：来定义边的编号（决定了在H矩阵中的位置） setMeasurement(type) 函数来定义观测值 setVertex(int, vertex) 来定义顶点 setInformation() 来定义协方差矩阵的逆 2.7 设置优化参数 设置优化参数，开始优化； 第7步 optimizer.initializeOptimization(); optimizer.optimize(100); //迭代100次
联系客服
