大局观之整体流程
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代码