SLAM练习题（十）——图优化G2O

SLAM练习题（⼗）——图优化G2O

SLAM学习笔记

⽂章⽬录

g20框架

在SLAM领域，基于图优化的⼀个⽤的⾮常⼴泛的库就是g2o，它是General Graphic Optimization 的简称，是⼀个⽤来优化⾮线性误差函数的c++框架。

求解过程：

先指定求解⽅法：OptimizationWithHessian 内部包含⼀个求解器（Solver），这个Solver实际是由⼀个BlockSolver组成的。==这个BlockSolver有两个部分，⼀个是SparseBlockMatrix ，⽤于计算稀疏的雅可⽐和Hessian矩阵；⼀个是线性⽅程的求解器（LinearSolver），==它⽤于计算迭代过程中最关键的⼀步HΔx=−b，LinearSolver有⼏种⽅法可以选择：PCG, CSparse, Choldmod。

再添加顶点和边：注意看 has-many 箭头，这个超图包含了许多顶点（HyperGraph::Vertex）和边（HyperGraph::Edge）。⽽这些顶点顶点继承⾃ Base Vertex，也就是OptimizableGraph::Vertex，⽽边

可以继承⾃ BaseUnaryEdge（单边）, BaseBinaryEdge（双边）或BaseMultiEdge（多边），它们都叫做OptimizableGraph::Edge。添加好定点和边后，设置优化参数，开始执⾏优化。

步骤：

Tip :g2o是⽤来优化⾮线性误差函数的，它的求解⽅法与优化变量的维度类型和误差的维度类型息息相关

typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1>> Block;// 每个误差项优化变量维度为3，误差值维度为1

// 第1步：创建⼀个线性求解器LinearSolver

Block::LinearSolverType* linearSolver =new g2o::LinearSolverDense<Block::PoseMatrixType>();

// 第2步：创建BlockSolver。并⽤上⾯定义的线性求解器初始化

Block* solver_ptr =new Block( linearSolver );

// 第3步：创建总求解器solver。并从GN, LM, DogLeg 中选⼀个，再⽤上述块求解器BlockSolver初始化

g2o::OptimizationAlgorithmLevenberg* solver =new g2o::OptimizationAlgorithmLevenberg( solver_ptr );

// 第4步：创建终极⼤boss 稀疏优化器（SparseOptimizer）

g2o::SparseOptimizer optimizer;// 图模型

optimizer.setAlgorithm( solver );// 设置求解器

optimizer.setVerbose(true);// 打开调试输出

// 第5步：定义图的顶点和边。并添加到SparseOptimizer中

CurveFittingVertex* v =new CurveFittingVertex();//往图中增加顶点

v->setEstimate( Eigen::Vector3d(0,0,0));

v->setId(0);

optimizer.addVertex( v );

for(int i=0; i<N; i++)// 往图中增加边

{

CurveFittingEdge* edge =new CurveFittingEdge( x_data[i]);

edge->setId(i);

edge->setVertex(0, v );// 设置连接的顶点

edge->setMeasurement( y_data[i]);// 观测数值

edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma));// 信息矩阵：协⽅差矩阵之逆

optimizer.addEdge( edge );

}

// 第6步：设置优化参数，开始执⾏优化

optimizer.initializeOptimization();

optimizer.optimize(100);

求解步骤

1创建⼀个线性求解器

我们要求的增量⽅程的形式是：H△X=-b，通常情况下想到的⽅法就是直接求逆，也就是△X=-H.inv*b。看起来好像很简单，但这有个前提，就是H的维度较⼩，此时只需要矩阵的求逆就能解决问题。但是当H的维度较⼤时，矩阵求逆变得很困难，求解问题也变得很复杂。此时我们就需要⼀些特殊的⽅法对矩阵进⾏求逆。

g2o提供的⼏种线性求解⽅法：

LinearSolverCholmod ：使⽤sparse cholesky分解法。继承⾃LinearSolverCCS

LinearSolverCSparse：使⽤CSparse法。继承⾃LinearSolverCCS

LinearSolverPCG ：使⽤preconditioned conjugate gradient 法，继承⾃LinearSolver

LinearSolverDense ：使⽤dense cholesky分解法。继承⾃LinearSolver

LinearSolverEigen：依赖项只有eigen，使⽤eigen中sparse Cholesky 求解，因此编译好后可以⽅便的在其他地⽅使⽤，性能和CSparse差不多。继承⾃LinearSolver。

矩阵分解参考

PCG算法参考

2创建BlockSolver

BlockSolver 内部包含 LinearSolver，⽤上⾯我们定义的线性求解器LinearSolver来初始化。它的定义在如下⽂件夹内：

g2o/g2o/core/block_solver.h

在定义中可以发现，BlockSolver有两种定义⽅式：

⼀种是指定的固定变量的solver，我们来看⼀下定义：

using BlockSolverPL = BlockSolver< BlockSolverTraits<p, l>>;

其中p代表pose的维度（注意⼀定是流形manifold下的最⼩表⽰），l表⽰landmark的维度。

⼀般情况下，位姿和路标的维度我们都是知道的，所以这个⽤的⽐较多。

另⼀种是可变尺⼨的solver，定义如下

using BlockSolverX = BlockSolverPL<Eigen::Dynamic, Eigen::Dynamic>;

可变尺⼨的solver:

在某些应⽤场景，我们的Pose和Landmark在程序开始时并不能确定，那么此时这个块状求解器就没办法固定变量，此时使⽤这个可变尺⼨的solver，所有的参数都在中间过程中被确定。

3创建总求解器solver

创建总求解器solver。并从GN, LM, DogLeg 中选⼀个，再⽤上述块求解器BlockSolver初始化。

在g2o/g2o/core/ ⽬录下，发现Solver的优化⽅法有三种：分别是⾼斯⽜顿（GaussNewton）法，LM（Levenberg–Marquardt）法、Dogleg法，他们都继承⾃同⼀个类：OptimizationWithHessian，该类⼜继承⾃OptimizationAlgorithm。

三种迭代策略：美国丽人下载

g2o::OptimizationAlgorithmGaussNewton

g2o::OptimizationAlgorithmLevenberg

作文黑板上的记忆

g2o::OptimizationAlgorithmDogleg

4创建稀疏优化器（SparseOptimizer）

g2o::SparseOptimizer optimizer;// 创建稀疏优化器

SparseOptimizer::setAlgorithm(OptimizationAlgorithm* algorithm)// ⽤前⾯定义好的求解器作为求解⽅法

SparseOptimizer::setVerbose(bool verbose)// 设置优化过程输出信息

5定义顶点和边并添加

顶点

对于顶点，g2o中提供了⼀个⽐较通⽤的适合⼤部分情况的模板：BaseVertex（参考上⾯的g2o框架图），它在

g2o/core/base_vertex.h：(这个⽂件相对来说较⼩，直接放上来了)

#ifndef G2O_BASE_VERTEX_H

#define G2O_BASE_VERTEX_H

#include"optimizable_graph.h"

#include"creators.h"

#include"g2o/stuff/macros.h"

#include<Eigen/Core>

#include<Eigen/Dense>

#include<Eigen/Cholesky>

#include<Eigen/StdVector>

#include<stack>

namespace g2o {

/**

* \brief Templatized BaseVertex

* Templatized BaseVertex

* D : minimal dimension of the vertex, e.g., 3 for rotation in 3D

* T : internal type to represent the estimate, e.g., Quaternion for rotation in 3D

template<int D,typename T>

class BaseVertex :public OptimizableGraph::Vertex {

public:

typedef T EstimateType;

typedef std::stack<EstimateType,

std::vector<EstimateType, Eigen::aligned_allocator<EstimateType>>>

我的军校生活

BackupStackType;

static const int Dimension = D;///< dimension of the estimate (minimal) in the manifold space

typedef Eigen::Map<Eigen::Matrix<number_t, D, D, Eigen::ColMajor>, Eigen::Matrix<number_t, D, D, Eigen::ColMajor>::Flags & Eigen::PacketAccessBit Eigen::Aligned : Eigen::Unaligned > HessianBlockType;

public:

BaseVertex();

virtual const number_t&hessian(int i,int j)const{assert(i<D && j<D);return_hessian(i,j);}

virtual number_t&hessian(int i,int j){assert(i<D && j<D);return_hessian(i,j);}

幽灵楼道virtual number_t hessianDeterminant()const{return _hessian.determinant();}

virtual number_t*hessianData(){return const_cast<number_t*>(_hessian.data());}

inline virtual void mapHessianMemory(number_t* d);

virtual int copyB(number_t* b_)const{

memcpy(b_, _b.data(), Dimension *sizeof(number_t));

return Dimension;

}

virtual const number_t&b(int i)const{assert(i < D);return_b(i);}

virtual number_t&b(int i){assert(i < D);return_b(i);}

virtual number_t*bData(){return _b.data();}

inline virtual void clearQuadraticForm();

//! updates the current vertex with the direct solution x += H_ii\b_ii

/! @returns the determinant of the inverted hessian

inline virtual number_t solveDirect(number_t lambda=0);

//! return right hand side b of the constructed linear system

Eigen::Matrix<number_t, D,1, Eigen::ColMajor>&b(){return _b;}

const Eigen::Matrix<number_t, D,1, Eigen::ColMajor>&b()const{return _b;}

//! return the hessian block associated with the vertex

HessianBlockType&A(){return _hessian;}

const HessianBlockType&A()const{return _hessian;}

virtual void push(){ _backup.push(_estimate);}

virtual void pop(){assert(!_pty()); _estimate = _p(); _backup.pop();updateCache();}

virtual void discardTop(){assert(!_pty()); _backup.pop();}

virtual int stackSize()const{return _backup.size();}

//! return the current estimate of the vertex

const EstimateType&estimate()const{return _estimate;}

//! set the estimate for the vertex also calls updateCache()

void setEstimate(const EstimateType& et){ _estimate = et;updateCache();}

protected:

HessianBlockType _hessian;

Eigen::Matrix<number_t, D,1, Eigen::ColMajor> _b;

黄业斌EstimateType _estimate;

BackupStackType _backup;

public:

EIGEN_MAKE_ALIGNED_OPERATOR_NEW

};

#include"base_vertex.hpp"

}// end namespace g2o

#endif

可以看到，BaseVertex是个抽象类，我们在使⽤的时候需要重写其中的虚函数。

D并⾮是顶点（更确切的说是状态变量）的维度，⽽是其在流形空间（manifold）的最⼩表⽰，这⾥⼀定要区别开，T就是顶点（状态变量）的类型。

定义顶点

不同的应⽤场景（⼆维空间，三维空间），有不同的待优化变量（位姿，空间点），还涉及不同的优化类型（李代数位姿、李位

姿），g2o本⾝内部定义了⼀些常⽤的顶点类型：

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>

上⾯的这些顶点我们可以直接⽤，但是有时候我们需要的顶点类型这⾥⾯没有，就得⾃⼰定义了。

重新定义顶点⼀般需要考虑重写如下函数：

virtual bool read(std::istream& is);

virtual bool write(std::ostream& os)const;

virtual void oplusImpl(const number_t* update);

virtual void setToOriginImpl();德隆系

**read，write：**分别是读盘、存盘函数，⼀般情况下不需要进⾏读/写操作的话，仅仅声明⼀下就可以

setToOriginImpl：顶点重置函数，设定被优化变量的原始值。

**oplusImpl：**顶点更新函数。⾮常重要的⼀个函数，主要⽤于优化过程中增量△x 的计算。我们根据增量⽅程计算出增量之后，就是通过这个函数对估计值进⾏调整的，因此这个函数的内容⼀定要重视。

⾃⼰定义顶点的⼀般格式：

本文发布于:2024-09-22 05:25:34，感谢您对本站的认可！

本文链接：https://www.17tex.com/xueshu/53835.html

上一篇：从零开始一起学习SLAM理解图优化，一步步带你看懂g2o代码

下一篇：LASSO回归

标签：优化顶点求解函数

留言与评论（共有 0 条评论）