本人讲解关于slam一系列文章汇总链接:史上最全slam从零开始
文末正下方中心提供了本人 联系方式, 点击本人照片即可显示 W X → 官方认证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式,\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式,点击本人照片即可显示WX→官方认证
一、前言
在上一篇博客中,讲解了运行该示例代码环境的搭建,同时对 g2o 进行了简单的讲解。其实对于g2o(图优化)来说,只要理解了 顶点 ( V e r t e x ) \color{red}顶点 (Vertex) 顶点(Vertex) 与 边 ( E d g e ) \color{red}边(Edge) 边(Edge) ,那么接下下来的构图以及编程都是十分的简单,所以在进行代码讲解之前,先来看看 顶点 (Vertex) 与 边(Edge) 的介绍。该篇文章主要介绍顶点 Vertex,Edge 下篇文章中进行介绍。 主要分为以下几个部分:1、g2o的顶点(Vertex) 从哪里来的;2、g2o的顶点(Vertex) 参数如何理解;3、如何自己定义顶点;4、如何向图中添加顶点。这里先粘贴一下类的结构图:
二、g2o的顶点(Vertex) 从哪里来的
在源码 https://github.com/RainerKuemmerle/g2o 的 doc 目录中,可以看到一下 g2o.pdf 文件,该为教材文件,想深入了解的朋友可以认真查看。其上,上图也是来自于该文档。我们先来看其中关于顶点的这一部分,如下:
首先看到上面的类①HyperGraph::Vertex,其位于源码的 g2o/core/hyper_graph.h 文件中:
class G2O_CORE_API HyperGraph
{
public:
......
......
//! abstract Vertex, your types must derive from that one
class G2O_CORE_API Vertex : public HyperGraphElement {
......
......
};
上的的代码表示 class HyperGraph 包含了 class Vertex ,并且 Vertex 继承于 HyperGraphElement。另外通过注释,可以知道 class Vertex 是一个抽象类,上图②OptimizableGraph::Vertex 由其派生而来,或者说 ②OptimizableGraph::Vertex 继承于 HyperGraph::Vertex。如下所示,在 core/optimizable_graph.h 文件中,可以看到如下代码:
struct G2O_CORE_API OptimizableGraph : public HyperGraph {
......
......
/**
* \brief A general case Vertex for optimization
*/
class G2O_CORE_API Vertex : public HyperGraph::Vertex, public HyperGraph::DataContainer {
......
......
这里很明显可以看出来 OptimizableGraph::Vertex 继承于 HyperGraph::Vertex。不过,这个OptimizableGraph::Vertex 是比较底层的类,具体使用时一般会进行扩展,因此g2o中提供了一个比较通用的适合大部分情况的模板。也就是上图中的③BaseVertex<D, T>,其在 g2o/core/base_vertex.h 中实现,代码如下:
/**
* \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
......
......
这样就弄清楚了 Vertex 的来源,但是这个模板类 BaseVertex 如何理解呢?
三、g2o的顶点(Vertex) 参数如何理解?
因为常用的就是 class BaseVertex,所以我们对其进行讲解,其模板的参数分别为D 和 T,
1、D是int 类型的,表示vertex的最小维度,比如3D空间中旋转是3维的,那么这里 D = 3
2、T是待估计vertex的数据类型,比如用四元数表达三维旋转的话,T就是Quaternion 类型
class BaseVertex 源码中 D 是这样进行注释的:
static const int Dimension = D; ///< dimension of the estimate (minimal) in the manifold space
可以看到这个D并非是顶点(更确切的说是状态变量)的维度,而是其在流形空间(manifold)的最小表示,这里一定要区别开,另外,源码里面也给出了T的作用
typedef T EstimateType;
typedef std::stack<EstimateType,
可以看到,这里T就是顶点(状态变量)的类型,跟前面一样。
四、g2o的顶点(Vertex) 参数如何理解?
通过上面的了解,就可以写顶点的定义了,已知顶点的基本类型是 BaseVertex,那么下一步关心的就是如何使用了,因为在不同的应用场景(二维空间,三维空间),有不同的待优化变量(位姿,空间点),还涉及不同的优化类型(李代数位姿、李群位姿)。有这么多,是不是都要根据BaseVertex 自己一个个去实现呢?其实是不需要的,因为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();
每次定义都要重写这几个函数,来看一下他们都是什么意义:
( 1 ) r e a d , w r i t e : \color{blue} (1)read,write: (1)read,write:分别是读盘、存盘函数,一般情况下不需要进行读/写操作的话,仅仅声明一下就可以
( 2 ) s e t T o O r i g i n I m p l : \color{blue} (2)setToOriginImpl: (2)setToOriginImpl: 顶点重置函数,设定被优化变量的原始值。
( 3 ) o p l u s I m p l : \color{blue} (3)oplusImpl: (3)oplusImpl: 顶点更新函数。非常重要的一个函数,主要用于优化过程中增量 △ x △x △x 的计算。我们根据增量方程计算出增量之后,就是通过这个函数对估计值进行调整的,因此这个函数的内容一定要重视。
自己定义 顶点一般是下面的格式:
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*/;
}
}
先来看一个简单的例子,来自于
https://github.com/gaoxiang12/slambook2/blob/master/ch6/g2oCurveFitting.cpp
主要功能为曲线拟合,顶点编写如下:
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual void setToOriginImpl() // 重置
{
_estimate << 0,0,0;
}
virtual void oplusImpl( const double* update ) // 更新
{
_estimate += Eigen::Vector3d(update);
}
// 存盘和读盘:留空
virtual bool read( istream& in ) {
}
virtual bool write( ostream& out ) const {
}
};
该为曲线类型的顶点(后续有详细的代码讲解),模板参数→待优化变量的维度,以及数据类型。根据 _estimate << 0,0,0; 可以知道,顶点初始值为0,更新逻辑 _estimate += Eigen::Vector3d(update); 也比较简单,就是把更新量 update 加上去就行了。
对于这里例子来说是可以直接相加的,因为顶点类型是Eigen::Vector3d,属于向量,是可以通过加法来更新的。但是但是有些例子就不行,比如下面这个复杂点例子,如李代数表示位姿VertexSE3Expmap,来自于g2o/types/sba/types_six_dof_expmap.h:
/**
\* \brief SE3 Vertex parameterized internally with a transformation matrix
and externally with its exponential map
*/
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()); //更新方式
}
};
有的朋友看到可能比较懵逼,不过没有关系。那么 6, SE3Quat 分别是什么意思?其实在 slam十四讲 中都有提及到,书中是这样介绍的:
第一个参数 6 → \color{blue} 第一个参数6→ 第一个参数6→ 表示内部存储的优化变量维度,这是个6维的李代数
第二个参数 → \color{blue} 第二个参数→ 第二个参数→ 表示优化变量的类型,这里使用了g2o定义的相机位姿类型:SE3Quat(三维旋转,三维平移)
在g2o/types/slam3d/se3quat.h可以具体了解SE3Quat(),其内部使用了四元数表达旋转,然后加上位移来存储位姿,同时支持李代数上的运算,比如对数映射(log函数)、李代数上增量(update函数)等操作。那么这里更新时没有为什么像上面那样直接加上去?
对于位姿来说,是不能直接加的,原因是变换矩阵不满足加法封闭。为什么相机位姿顶点类VertexSE3Expmap使用了李代数表示相机位姿,而不是使用旋转矩阵和平移矩阵?其实也是上述原因的拓展:这是因为旋转矩阵是有约束的矩阵,它必须是正交矩阵且行列式为1。使用它作为优化变量就会引入额外的约束条件,从而增大优化的复杂度。而将旋转矩阵通过李群-李代数之间的转换关系转换为李代数表示,就可以把位姿估计变成无约束的优化问题,求解难度降低。继续来看一个例子:
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;
}
};
下面是三维点的例子,空间点位置 VertexPointXYZ,维度为3,类型是Eigen的Vector3,比较简单,就不解释了。
五、如何向图中添加顶点?
理解顶点 (Vertex)之后,对于把顶点 (Vertex)添加到图中是比较简单的,如 ch6/g2oCurveFitting.cpp 中的代码如下:
// 往图中增加顶点
CurveFittingVertex* v = new CurveFittingVertex();
v->setEstimate( Eigen::Vector3d(0,0,0) );
v->setId(0);
optimizer.addVertex( v );
其中的setId(int) 定义节点编号,这个主要是后续获取数据,以及顶点与边连接时需要使用到。下面是添加 VertexSBAPointXYZ 的例子,都很容易看懂,位于/ch7/pose_estimation_3d2d.cpp之中:
int index = 1;
for ( const Point3f p:points_3d ) // landmarks
{
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 );
}
六、结语
通过该篇博客,对于1、g2o的顶点(Vertex) 从哪里来的;2、g2o的顶点(Vertex) 参数如何理解;3、如何自己定义顶点;4、如何向图中添加顶点。四个环节已经讲解完成。下面来看看边(Edge)应该如何去理解与使用。