介绍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);
