大局观之整体流程
SLAM里，图优化一般可以分解为两个任务：
- 构建图。机器人位姿作为顶点，位姿间关系作为边。
- 优化图。调整机器人的位姿（顶点）来尽量满足边的约束，使得误差最小。
这里以g2o源码说明文件中的类图为基础，自己做了一个流程说明。
蓝色框可以说明图是如何构成的，绿色框中则是说明了一些求解时的基本信息。小太阳是点出了整个类图最中心的部分SparseOptimizer，而橙色的箭头则是大概指出了写代码的流程。
理解了流程之后，就可以对照一下代码：
typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block; // 每个误差项优化变量维度为3，误差值维度为1
// 第1步：创建一个线性求解器LinearSolver,有以下五种：
// LinearSolverCholmod ：使用sparse cholesky分解法。继承自LinearSolverCCS
// LinearSolverCSparse：使用CSparse法。继承自LinearSolverCCS
// LinearSolverPCG ：使用preconditioned conjugate gradient 法，继承自LinearSolver
// LinearSolverDense ：使用dense cholesky分解法。继承自LinearSolver
// LinearSolverEigen： 依赖项只有eigen，使用eigen中sparse Cholesky 求解，因此编译好后可以方便的在其他地方使用，性能和CSparse差不多。继承自LinearSolver
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
// 第2步：创建BlockSolver。并用上面定义的线性求解器初始化
// BlockSolver有两种定义方式，由使用场景决定
// 1)固定变量solver,定义为：using BlockSolverPL = BlockSolver< BlockSolverTraits<p, l> >;
// p表示pose在流形下的最小表示，l表示landmark的维度
// 2)可变尺寸solver，有时p和l不能一开始确定，定义为：
// using BlockSolverX = BlockSolverPL<Eigen::Dynamic, Eigen::Dynamic>;
// 常用类型如下：
// BlockSolver_6_3 ：表示pose 是6维，观测点是3维。用于3D SLAM中的BA
// BlockSolver_7_3：在BlockSolver_6_3 的基础上多了一个scale
// BlockSolver_3_2：表示pose 是3维，观测点是2维
Block* solver_ptr = new Block( linearSolver );
// 第3步：创建总求解器solver，选择一种优化算法，再用上述块求解器BlockSolver初始化
// 优化算法有3种： 高斯牛顿GN, LM, DogLeg
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
// 第4步：创建稀疏优化器（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); //100次迭代
抠细节
顶点Vertex
关于顶点的内容主要按照顺序从三个方面逐步深入：
- 从源码入手理解顶点
Vertex及其参数 - 怎么自己定义顶点？
- 怎么把顶点加入图中
A. 理解顶点Vertex及其参数
首先，我们可以从上面的类图中截取出关于顶点的部分，牢记is a箭头指向的是基类，也可以通过查看源码清晰的看到继承关系。
注意这里的BaseVertex<D,T>中参数的含义：
| 参数 | 含义 |
|---|---|
| D | 状态变量(顶点)在其流形空间(manifold)的最小表示，并不是指顶点的维度 |
| T | 待估计状态变量(顶点)的数据类型，若用四元数表旋转，则T为Quaternion |
相当于说，顶点的基本类型是BaseVertex，我们之后的对顶点进行定义也是围绕这个类展开的。
B. 怎么定义顶点？
g2o中的预定义顶点
常用的一些顶点类型，可以直接用g2o内部的定义：
//2D pose Vertex, (x,y,theta)
VertexSE2 : public BaseVertex<3, SE2>
//6d vector (x,y,z,qx,qy,qz) 注意这里省略了四元数的实部w
VertexSE3 : public BaseVertex<6, Isometry3>
VertexPointXY : public BaseVertex<2, Vector2>
VertexPointXYZ : public BaseVertex<3, Vector3>
VertexSBAPointXYZ : public BaseVertex<3, Vector3>
// SE3 Vertex 内部用一个转换矩阵(T)参数化，外部用该矩阵的指数映射参数化
VertexSE3Expmap : public BaseVertex<6, SE3Quat>
// SBACam Vertex, (x,y,z,qw,qx,qy,qz),(x,y,z,qx,qy,qz)
// qw假定为正，否则用qx,qy,qz表示旋转时会导致模糊表示
VertexCam : public BaseVertex<6, SBACam>
// Sim3 Vertex, (x,y,z,qw,qx,qy,qz),7d vector,(x,y,z,qx,qy,qz)
VertexSim3Expmap : public BaseVertex<7, Sim3>
自定义顶点
重新定义顶点需要考虑重写(override) 下面的函数:
// 读/写函数，一般情况下不需要进行这两个操作，声明一下即可
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
// 顶点更新函数，非常重要！主要用于增量△x的计算。
// 当根据增量方程算出增量后，就要通过这个函数对估计值进行调整
virtual void oplusImpl(const number_t* update);
// 顶点重置函数，设定待优化量的初始值
virtual void setToOriginImpl();
自定义顶点，就相当于要以BaseVertex为基类，再写一个自它继承下来的新顶点类。先看具体的例子，然后再把这个具体的例子抽象成代码模板。
例子1：slam十四讲中的曲线拟合 ch6/g2o_curve_fitting/main.cpp
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// 把顶点的初值设置为0,0,0
virtual void setToOriginImpl()
{
_estimate << 0,0,0;
}
// 更新，相当于x = x + △x, 这里顶点类型为Eigen::Vector3d可以直接相加
virtual void oplusImpl( const double* update )
{
_estimate += Eigen::Vector3d(update);
}
// 存盘和读盘
virtual bool read( istream& in ) {}
virtual bool write( ostream& out ) const {}
};
例子2：三维点作为待优化变量
class G2O_TYPES_SBA_API VertexSBAPointXYZ : public BaseVertex<3, Vector3>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
VertexSBAPointXYZ();
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
virtual void setToOriginImpl() {
_estimate.fill(0);
}
virtual void oplusImpl(const number_t* update)
{
Eigen::Map<const Vector3> v(update);
_estimate += v;
}
};
例子3：g2o源码 g2o/types/sba/types_six_dof_expmap.h
/*
brief SE3 Vertex parameterized internally with a transformation matrix
and externally with its exponential map
*/
// 6是优化变量维度，即6维李代数
// SE3Quat是相机位姿类型，内部使用四元数表示旋转，再加上位移来表示位姿
// 同时支持李代数上的运算，如对数映射，增量操作等
class G2O_TYPES_SBA_API VertexSE3Expmap : public BaseVertex<6, SE3Quat>{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
VertexSE3Expmap();
bool read(std::istream& is);
bool write(std::ostream& os) const;
virtual void setToOriginImpl() {
_estimate = SE3Quat();
}
virtual void oplusImpl(const number_t* update_) {
Eigen::Map<const Vector6> update(update_);
// 更新方式
setEstimate(SE3Quat::exp(update)*estimate());
}
};
为什么此处更新不是直接相加？
因为变换矩阵不满足加法封闭。
为什么相机位姿顶点类的VertexSE3Expmap使用李代数表示相机位姿，而没有使用旋转矩阵和平移矩阵？
因为旋转矩阵是有约束的矩阵，它必须是正交矩阵且行列式为1。所以，如果要用它作为优化变量就会引入额外的约束条件，从而增大优化的复杂度。而将旋转矩阵通过李群-李代数之间的转换关系转换为李代数表示后，就可以把位姿估计变成无约束的优化问题，求解难度降低。
抽象后的代码模板：
class myVertex: public g2::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();
}
virtual void oplusImpl(const double* update) override
{
_estimate += /*update*/;
}
}
C. 怎么把顶点加入图中
上接slam十四讲中的曲线拟合的例子1：
// 往图中增加顶点
CurveFittingVertex* v = new CurveFittingVertex();
v->setEstimate( Eigen::Vector3d(0,0,0) ); //设置初始估计值
v->setId(0); //定义节点编号
optimizer.addVertex( v ); //optimizer是稀疏优化器的对象
接着上面一点中优化三维点的例子2：
int index = 1;
// 遍历每一个landmark
for ( const Point3f p:points_3d )
{
//每一次都新建一个顶点对象
g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
point->setId ( index++ );
point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );
point->setMarginalized ( true ); //边缘化
optimizer.addVertex ( point );
}
边Edge
边的内容也按照顺序从三个方面逐步深入：
- 理解边及其参数
- 怎么自己定义边？
- 怎么把边加入图中
A. 理解边及其参数
边的继承关系和顶点类似，只是这里有三个通用的模板：一元边、二元边、多元边。关于多元边的使用，参考github上的这个issue：#165
这里对几个参数进行说明：
| 参数 | 含义 |
|---|---|
| D | 为int类型，表示测量值的维度 |
| E | 测量值的数据类型 |
VertexXi VertexXj | 不同顶点的类型 |
举例：用边表示三维点投影到图像平面的重投影误差
// 二元边
// 测量值的维度为2，其数据类型是Vector2D，
// 连接这条边的两个顶点类型，一个是三维点，一个是李群位姿
BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>
B. 怎么定义边？
和顶点类似，定义边的时候也要重写一些成员函数：
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
// 使用当前顶点的值计算估计值和测量值之间的误差
virtual void computeError();
// 在当前顶点的值下，该误差对待优化变量的偏导数，即Jacobian
virtual void linearizeOplus();
// 其他重要的成员变量和函数
_measurement：存储观测值
_error：存储computeError() 函数计算的误差
_vertices[]：存储顶点信息，如二元边的_vertices[] 的大小为2，存储顺序和调用setVertex(int, vertex) 是设定的int 有关（0 或1）
setId(int)：来定义边的编号（决定了在H矩阵中的位置）
setMeasurement(type) 函数来定义观测值
setVertex(int, vertex) 来定义顶点
setInformation() 来定义协方差矩阵的逆
这里同样先举具体的例子，再写出抽象的代码流程。
例子1：slam十四讲中的曲线拟合 ch6/g2o_curve_fitting/main.cpp
// 一元边
// 模板参数：观测值维度，测量值类型，连接顶点类型
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
// 计算曲线模型误差
void computeError()
{
const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
const Eigen::Vector3d abc = v->estimate();
// 误差函数
_error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ;
}
virtual bool read( istream& in ) {}
virtual bool write( ostream& out ) const {}
public:
double _x; // x 值， y 值为 _measurement
};
例子2：3D-2D点的PnP 问题，即最小化重投影误差问题 g2o/types/sba/types_six_dof_expmap.h
※这里定义的时候一定要特别注意，哪个顶点对应的_vertices[0],哪个对应的_vertices[1], 之后把边添加到图中时，即edge->setVertex()，就要遵循这个顺序！
//继承了BaseBinaryEdge类，观测值是2维，类型Vector2D,顶点分别是三维点、李群位姿
class G2O_TYPES_SBA_API EdgeProjectXYZ2UV : public BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
//1. 默认初始化
EdgeProjectXYZ2UV();
//2. 计算误差
void computeError() {
//李群相机位姿v1 注意是对应_vertices[1]
const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
// 顶点v2 注意是对应_vertices[0]
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);
//cam_map()用于把相机坐标系下三维点用内参转换为图像坐标
//v1->estimate().map()表示把世界坐标系下三维点变换到相机坐标系
_error = obs-cam->cam_map(v1->estimate().map(v2->estimate()));
}
//3. 线性增量函数，也就是雅克比矩阵J的计算方法
// 详细内容参考slam14讲的第七讲 公式7.45-47
virtual void linearizeOplus();
//4. 相机参数
CameraParameters * _cam;
bool read(std::istream& is);
bool write(std::ostream& os) const;
};
C. 向图添加边
例子1：曲线拟合添加一元边 slambook/ch6/g2o_curve_fitting/main.cpp
// 往图中增加边
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] ); // 观测数值就是实际得到的数据点y坐标
// 信息矩阵：协方差矩阵的逆
edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) );
optimizer.addEdge( edge );
}
例子2：pnp优化添加二元边 slambook/ch7/pose_estimation_3d2d.cpp
对照B中的例子2一起看
index = 1;
for ( const Point2f p:points_2d )
{
g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
edge->setId ( index );
//这里对边连接的两个顶点进行设置，v[0]是三维点，v[1]是位姿，不能调换顺序！要和之前定义的顺序统一
edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );
edge->setVertex ( 1, pose );
// 这里的测量值是特征点的像素坐标
edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );
edge->setParameterId ( 0,0 );
edge->setInformation ( Eigen::Matrix2d::Identity() );
optimizer.addEdge ( edge );
index++;
}
\ 菜鸟起飞篇结束
参考资料：
从零开始一起学习SLAM | 掌握g2o顶点编程套路
从零开始一起学习SLAM | 掌握g2o边的代码套路
从零开始一起学习SLAM | 理解图优化，一步步带你看懂g2o代码
9386
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
