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

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

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

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

---

下面是专栏地址：

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

---

---

前言

这个系列的文章是分享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 one
  class 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 optimization
	class 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_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 += /*更新*/；
  }
}

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

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

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_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”表示内部存储的优化变量维度，这是一个六维的李代数。

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

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

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

三维点示例

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

// 以三维点作为顶点的定义示例
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;		// 加法更新
  }
}

如何向图中添加顶点

// 在曲线拟合中向图中添加顶点示例
// 新建顶点
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 );
}