SLAM算法与工程实践系列文章

下面是SLAM算法与工程实践系列文章的总链接，本人发表这个系列的文章链接均收录于此

SLAM算法与工程实践系列文章链接

---

下面是专栏地址：

SLAM算法与工程实践系列专栏

---

文章目录

• SLAM算法与工程实践系列文章
• • SLAM算法与工程实践系列文章链接
  • SLAM算法与工程实践系列专栏
• 前言
• SLAM算法与工程实践——SLAM基本库的安装与使用（6）：g2o优化库（3）
• • 如何自己定义顶点
  • • 一般格式
    • 在曲线拟合中自定义顶点的例子
    • SE(3)位姿示例
    • 三维点示例
  • 如何向图中添加顶点

---

前言

这个系列的文章是分享SLAM相关技术算法的学习和工程实践

---

SLAM算法与工程实践——SLAM基本库的安装与使用（6）：g2o优化库（3）

在图6-2中，涉及顶点的3个类是 HyperGraph::Vertex、OptimizableGraph::Vertex 和 BaseVertex。

先来看第1个类——HyperGraph::Vertex，它是一个抽象顶点类，必须通过派生来使用。下面是其定义中的说明。

// hyper graph.h
class G2O CORE API HyperGraph
{public:// ......//! abstract Vertex,your types must derive from that oneclass G2O_CORE_API Vertex : public yperGraphElement{// ......}
}

然后我们看第2个类——OptimizableGraph::Vertex，查看定义可以发现它继承自HyperGraph::Vertex，如下所示。

// optimizable graph.h
struct G2O_CORE_API OptimizableGraph : public HyperGraph
{// ......//A general case Vertex for optimizationclass G2O_CORE_API Vertex : public HyperGraph::Vertex,public
HyperGraph::DataContainer{// ......}
}

不过，OptimizableGraph::Vertex 也是非常底层的类，在具体使用时一般都会进行扩展，因此 g2o 提供了一个比较通用的适合大部分情况的模板，也就是第 3个类一BaseVertex。

我们找到源码中关于 BaseVertex 的定义，可以发现 BaseVertex 继承 OptimizableGraph::Vertex。

以上 3 个类的关系和 图 6-2 中显示的完全一致。

// g2o/core/base vertex.h
namespace g2o
{
#define G2O_VERTEX_DIM ((D == Eigen:Dynamic) ? _dimension : D)
/**
*	\brief Templatized Basevertex
*	Templatized Basevertex
*	D : minimal dimension of the vertex,e.g.,3 for rotation in 3D.-1 means dynamically assigned at runtime.
*	T : internal type to represent the estimate,e.g.,Quaternion for rotation in 3D
*/template <int D,typename T>class Basevertex : public OptimizableGraph::Vertex{static const int Dimension D;// dimension of the estimate (minimal)in the manifold space// ......}
}

最后我们来看上述代码中的模板参数D和T。

D是 int 类型的，表示 Vertex 的最小维度，比如在3D空间中旋转是三维的，那么这里D=3。

T是待估计 Vertex 的数据类型，比如用四元数表达三维旋转，那么 T 就是 Quaternion 类型的。

如何自己定义顶点

g2o内部定义了一些常用的顶点类型

// g2o 定义好的常用顶点类型
// 2D位姿顶点(x,y,theta)
VertexSE2 : public Basevertex<3,SE2>// 六维向量(x,y,z,gx,qy,gz),省略了四元数中的qw
VertexSE3 : public Basevertex<6,Isometry3>// 二维点和三维点
VertexPointXY : public BaseVertex<2,Vector2>
VertexPointXYZ : public Basevertex<3,Vector3>
VertexSBAPointXYZ : public Basevertex<3,Vector3>// SE(3)顶点，内部用变换矩阵参数化，外部用指数映射参数化
VertexSE3Expmap : public BaseVertex<6,SE3Quat>// SBACam顶点
VertexCam : public Basevertex<6,SBACam>// Sim(3)顶点
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();

一般格式

一般用下面的格式自定义g2o顶点

// 自定义 g2o 顶点通用格式
class myVertex:public g2o::BaseVertex<Dim,Type>
{public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWmyvertex(){}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 += /*更新*/；}
}

在曲线拟合中自定义顶点的例子

// 在曲线拟合中自定义顶点
class CurveFittingVertex:public g2o::BaseVertex<3,Eigen:Vector3d>
{public:EIGEN MAKE ALIGNED OPERATOR NEW// 设定被优化顶点的初始值为0virtual 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 {}
};

SE(3)位姿示例

以SE(3)位姿作为顶点的定义示例。更新函数用的就是乘法

// 以SE(3)位姿作为顶点的定义示例
// g2o/types/sba/types_six_dof_expmap.h
class g2o TYPES SBA API VertexSE3Expmap : public Basevertex<6,SE3Quat>{public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWVertexSE3Expmap();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”表示内部存储的优化变量维度，这是一个六维的李代数。

第二个参数是优化变量的类型，这里使用了g2o 定义的相机位姿类型 SE3Quat。SE3Quat内部使用了四元数表达旋转，然后加上位移来存储SE(3)位姿。

为什么这里更新时没有直接做加法呢？

这里不用加法来更新位姿是因为SE(3)位姿不满足加法封闭性，但它对乘法是封闭的。

三维点示例

以三维点作为顶点的定义示例

// 以三维点作为顶点的定义示例
class g2o_TYPES_SBA_API VertexSBAPointXYZ : public Basevertex<3,Vector3>
{public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWVertexSBAPointXYZ();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;		// 加法更新}
}

如何向图中添加顶点

// 在曲线拟合中向图中添加顶点示例
// 新建顶点
CurveFittingVertex* v = new CurveFittingvertex();
//设定估计值
v->setEstimate(Eigen::Vector3d(0,0,0));
//设置顶点编号ID
v->setId(0);
//将顶点添加到优化器中
optimizer.addVertex( v );

下面是添加 VertexSBAPointXYZ 顶点的例子。

// 顶点初始 ID 为 1
int index = 1;
// 循环添加所有三维点并作为顶点
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 );
}