一、总体介绍
g2o(General Graphic Optimization)是一个通用图优化算法库,目前主流的SLAM研究基本都是基于优化的,可以在此处查阅其论文原文。
图优化中的点是相机位姿,边是位姿之间的变换关系,通常表示误差项。
g2o的类关系图
从整个结构框图分析,稀疏优化器SparseOptimizer是整个g2o的核心。
二、搭建步骤
要使用g2o求解优化问题,框架的搭建步骤如下:
(1)创建一个线性求解器 LinearSolver。
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
g2o提供的求解方式主要有5种,
继承自 LinearSolverCCS 的有2种:① LinearSolverCholmod,使用sparse cholesky分解法,继承自LinearSolverCCS;② LinearSolverCSparse,使用CSparse法,继承自LinearSolverCCS;
继承自 LinearSolver 的有3种:① LinearSolverPCG,使用preconditioned conjugate gradient 法,继承自LinearSolver。② LinearSolverDense,使用dense cholesky分解法,继承自LinearSolver。③ LinearSolverEigen,依赖项只有eigen,使用eigen中sparse Cholesky 求解,因此编译好后可以方便的在其他地方使用,性能和CSparse差不多,继承自LinearSolver。
(2)创建块求解器 BlockSolver,并使用上面定义的线性求解器初始化
typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block; // 每个误差项优化变量维度为3,误差值维度为1
Block* solver_ptr = new Block( linearSolver );
solver的定义有固定变量和可变尺寸两种,其定义方式分别为:
// 固定维度,p为pose的维度,l表示landmark的维度
using BlockSolverPL = BlockSolver< BlockSolverTraits<p, l> >;
// 可边尺寸
using BlockSolverX = BlockSolverPL<Eigen::Dynamic, Eigen::Dynamic>;
另外,g2o还预定义了以下3种常用类型:① BlockSolver_6_3,6维pose + 3维landmark,常用于Bundle Adjustment。 ② BlockSolver_7_3,在BlockSolver_6_3基础上多出一个scale维度。 ③ BlockSolver_3_2,3维pose + 2维lankmark,常用于平面世界。
(3)创建总求解器 solver,并从GN,LM,Dogleg中选一个,用上述块求解器BlockSolver初始化。
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
这里有三种迭代策略可供选择:g2o::OptimizationAlgorithmGaussNewton,g2o::OptimizationAlgorithmLevenberg,g2o::OptimizationAlgorithmDogleg。
(4)创建图优化的核心:稀疏优化器(SparseOptimizer)。
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm( solver ); // 设置求解方法
optimizer.setVerbose( true ); // 设置优化过程输出信息
(5)定义图的顶点和边,并添加到SparseOptimizer中
CurveFittingVertex* v = new CurveFittingVertex();
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); //设置迭代次数
三、图的顶点与边
3.1 顶点
g2o中定义Vertex有一个通用的类模板:BaseVertex,它有D/T两个参数,D是 int 类型,表示vertex的最小维度,T是待估计的 vertex 的数据类型,是状态在其流形空间中的最小表示。(存在疑问)
一些常用的顶点类型:
ertexSE2 : public BaseVertex<3, SE2> //2D pose Vertex, (x,y,theta)
VertexSE3 : public BaseVertex<6, Isometry3> //Isometry3使欧式变换矩阵T,实质是4*4矩阵//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 mapVertexSE3Expmap : 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 rotationVertexCam : 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>
当自己来定义顶点时,需要重写下面几个函数:
class myVertex: public g2o::BaseVertex<Dim, Type>
{
public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW
myVertex(){
}
// 读盘、存盘函数,一般情况下不需要进行读/写操作的话,仅仅声明一下就可以
virtual void read(std::istream& is) {
}
virtual void write(std::ostream& os) const {
}
// 顶点重置函数,设定被优化变量的原始值。
virtual void setOriginImpl(){
_estimate = Type(); }
// 顶点更新函数,主要用于优化过程中增量△x 的计算,根据增量方程计算出增量后,通过这个函数对估计值进行调整
virtual void oplusImpl(const double* update) override{
_estimate += update; }
}
将顶点添加到图中:
CurveFittingVertex* v = new CurveFittingVertex();
v->setEstimate( Eigen::Vector3d(0,0,0) );// 设定初始值v->setId(0); // 定义节点编号
optimizer.addVertex( v ); // 把节点添加到图中
3.2 边
g2o中定义的边有一元边、二元边、多元边,以二元边为例,它的参数有:D, E, VertexXi, VertexXj。D是 int 型,表示测量值维度;E表示测量值数据类型;VertexXi,VertexXj 分别表示不同顶点的类型。由此可以得到一个二元边的定义如下:
BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>
当自己来定义边时,需要重写下面几个函数:
class myEdge: public g2o::BaseBinaryEdge<errorDim, errorType, Vertex1Type, Vertex2Type>
{
public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW
myEdge(){
}
virtual bool read(istream& in) {
}
virtual bool write(ostream& out) const {
}
// 是使用当前顶点值计算的测量值与真实测量值之间的误差
virtual void computeError() override {
// ...
_error = _measurement - Something;
}
// 是在当前顶点的值下,该误差对优化变量的偏导数,也就是Jacobian矩阵
virtual void linearizeOplus() override // 求误差对优化变量的偏导数,雅克比矩阵
{
_jacobianOplusXi(pos, pos) = something; // ... /* _
jocobianOplusXj(pos, pos) = something; ... */
}
private:
data
}
参考博客: