【视觉SLAM】 G2O库编写步骤介绍

介绍G2O，并阐述基本使用方法。

G2O以稀疏优化器（SparseOptimizer）为核心，分为图的构建与求解器构建两部分，分别对应该图的上下两部分。

G2O编程步骤共分七步，如图所示：

构建求解器

1、创建一个线性求解器(LinearSolver)。

在此设定HΔx = -b的求解方法，一般采用LinearSolverDense：使用 dense cholesky 分解法。

2、创建块求解器(BlockSolver)，并用上面定义的线性求解器初始化。

using BlockSolverPL=BlockSolver< BlockSolverTraits<p，l>>;

在此设定误差项优化变量维度与误差值纬度。可选可变尺寸的求解器。

3、创建总求解器（Solver）,，并从 GN、LM、DogLeg 中选择一个，再用上述块求解器初始化。

4、创建稀疏优化器（Sparse0ptimizer），并用已定义求解器作为来解方法。

构建图结构

5、定义顶点以及顶点添加至优化器（Sparse0ptimizer）。

Vertex : public Basevertex< D，T>

顶点模板中的参数 D 和 T。

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

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

存在一些常用顶点：

2D位姿顶点(x，y, theta)

VertexSE2 : public Basevertex<3，SE2>

六维向量(x，y，z，qx，qy，qz)，省略了四元数中的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>

---

当所需定顶点不在其中时，需要重新定义顶点。重新定义顶点需要重写一下函数：

1、读/写函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以。

virtual bool read(std::istreams is);

virtual bool write(std::ostreams os) const;

2、顶点更新函数。这是一个非常重要的雨数，主要用于优化过程中增量Δx的计算。计算出增量后，就是通过这个函数对估计值进行调整的。

virtual void oplusImpl(const number t* update);

3、设定被优化顶点的初始值。

virtual void setToOriginImpl();

---

构建完成顶点后，需要向优化器（Sparse0ptimizer）中添加顶点。如下所示：

在曲线拟合中向图中添加顶点示例

新建顶点

CurveFittingVertex* v=new CurveFittingVertex();

设定估计值

v->setEstimate(Eigen::Vector3d(0,0,0));

//设置顶点编号ID

v->setId(0);

将顶点添加到优化器中

optimizer.addVertex(v);

6、定义边以及边添加至优化器（Sparse0ptimizer）。

BaseBinaryEdge<D,E, VertexXi, VertexXi >

D是int 类型的，表示测量值的维度(Dimension)。

E 表示测量值的数据类型。

VertexXi、VertexXi分别表示不同顶点的类型。

定义边时我们通常也需要复写一些要的成员函数。

读/写函数，一般情况下不需要进行读/写操作，仅声明一下就可以。

virtual bool read(std::istream& is);

virtual bool write(std::ostream& os) const;

使用当前顶点的值计算的测量值与真实的测量值之间的误差

virtual void computeError();

误差对优化变量的偏导数，也就是我们说的 Jacobianvirtual

void linearizeOplus();

---

定义完成边后，需要向优化器（Sparse0ptimizer）中添加边。如下所示：

 CurveFittingEdge* edge=new CurveFittingEdge(x data[i]);

//设置边的ID

edge->setId(i);

//设置连接的顶点v，其编号为0

edge->setVertex(0，v);

//设置观测的数值

edge->setMeasurement(ydata[i]);

// 信息矩阵

edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/ (wsigma*w sigma));

//将边添加到优化器

optimizer.addEdge( edge);

7、设置优化参数

//开始执行优化。

optimizer.initializeOptimization();

//设置迭代次数

optimizer.optimize(100);