宝塔服务器面板，一键全能部署及管理，送你10850元礼包，点我领取

g2o是一个通用的图表优化框架。

本文结合论文g2o : Ageneralframeworkfographoptimizaiton阐述g2o的理论，并编写样例程序说明G2O的使用方法。

g2o理论：

许多问题可以建模为最无聊鸭优化的状态估计问题。 提问形式如图所示

fx )是目标函数，以使目标函数最小化的方式求解状态x为目的。 x可以是向量，而向量中的每个元素可以是参数块。 zij是将状态变量I和状态变量j联系起来的观测，在数学上表现为一个约束方程式。 由于存在测量噪声，使用误差矢量e )评估xi和xj满足方程的程度。

论文中为了简化符号，使用下图记述误差向量

这里需要注意的是，状态变量xi和xj 左侧)为系统整体的状态x )右侧)。

该误差矢量可以表示为有向图，可以了解方程的结构，便于可视化分析

在这个图中，边的个数是所有的制约，也就是方程式的个数。 一个节点是一个状态变量，所有节点的耦合构成整个系统的状态变量。 别忘了我们的目标是为了状态估计。

有了上述目标函数，我们可以进行最无聊的鸭乘优化。

论文给出了高斯xfdcg方法。

高斯xfdcg法的思想是在误差函数e中采用一阶tsdc展开。 然后构造新的误差函数，求出该误差函数的极小值，以极小值作为新的迭代点。 重复直到满足结束条件。

这里需要注意的是，要求\delta x。

雅可比矩阵Jij为zij方程相对于状态变量x整体的雅可比，

方程14中关于h和b的参数是Jij。

也就是说，我们求解整个方程式的jacobi并带入方程式12，将b和h组合，最后求解H\delta x=-b。

论文还大致介绍了LM的方法，

线性方程中添加了具有衰减系数的单位数组。

这里不详细介绍。

最无聊的鸭子骑着SLAM的APP应用

空间刚体有6个自由度、3个旋转和3个平移。

平移位于欧式空间，但旋转位于非欧式空间。

表示旋转的方法有旋转矩阵、欧拉角、四元数和旋转向量。

欧拉角和旋转矢量是最小参数表示，但具有奇异性。

虽然旋转矩阵和四元数是过参数表示3个自由度，但是旋转矩阵9个，四元数4个参数)，但是有限制。

为了应用最无聊的鸭子骑行，可以表示\delta x最小参数。 因为\delta x通常比较小，远离奇点。

然后，我们采用了表示当前迭代点的参数。

定义了超帅的棒球操作。

论文表明，可以用归一化四元数表示增量，用整四元数表示迭代点，但这里还不清楚。

超帅的棒球和准星有关系。 这里，可以将两个旋转矩阵相乘，以表示相机当前的姿势、运动\delta x和以后的新姿势

新的误差函数格式如图所示

重要的杰克比如图，这就等于求导游

新的迭代点是

不管参数如何变化，h矩阵的结构都不会改变。

也就是说，各方程式中有bij和Hij，所有方程式的h和b构成系统的h和b。

注意： bij与三样东西有关。

假设有n个变量。 每个变量都是一个参数块。 假设只有一个自变量，则Jij为1Xn，\Omiga为1X1，e为1X1的向量。

同样是Hij=nX1X1X1X1Xn=nXn的矩阵。

也就是说，在h的ij、ji、ii中，自信的蝴蝶和b的I和j的位置不是零，其他的位置是0。

注意，h也是图的邻接矩阵。 因此，h的非零块与图中的边数成比例。 这引起了h的稀疏性。

有些问题可以用h的系数性求解线性系统。

在BA中，变量是照相机姿势p和道路标识点l

更改变量的顺序，然后单击、

通常，由于摄像机变量个数少，道路标识点变量的个数多，

通过采用h的舒尔候补，可以很简单。

Hll的逆是对角数组，便于求解。 然后，如果方程式的个数减少，就可以求解\delta x_p

然后，可以求出\delta x_l

实现方法：

总结：

计算每次观测的误差矢量

计算误差向量的雅可比；

更新系统的h和b

然后求解线性系统

应用：

使用g2o时，需要用优化的思想编写程序。

图由节点和边构成，首先用类的形式定义节点和边。

顶点可以继承BaseVertex模板类，该类是顶点的最小参数数量和顶点的数据类型。

我们需要实现的是田操作，改写virtualvoidoplusimpl const double * update )函数。

估计值存储在变量_estimate中； 函数void setToOriginImpl ) )重置节点的值。 定义节点时，利用setestimatetype )函数设定初始值； setidint )定义节点编号。

p>边也是继承一个模板类，模板参数为误差向量e的维度，观测z的数据类型和边连接的顶点类型。这里需要注意的是，几个顶点就是几元边。
边也就是上文中的eij，我们需要为边定义误差函数void computeError)
定义雅克比Jij void linearizeOplus) ,观测值存储在_measurement 中，误差存储在_error 中，节点存储在_vertices[] 。定义边时，利用setIdint) 来定义边的编号（决定了在H矩阵中的位置）；setMeasurementtype) 函数来定义观测值；setVertexint, vertex) 来定义节点；setInformation) 来定义协方差矩阵的逆。

定义完顶点和边后，我们就可以构建这个图，并对其优化。

定义一个图优化的对象 g2o::SparseOptimizer optimizer;
设置一些图优化相关的内容，
也就是有了线性系统H\deltax=-b后，我们还需要设定求解线性系统的方法。
线性系统可以是带阻尼的LM方法，这就需要我们指定用哪个优化算法
g2o::OptimizationAlgorithmLevenberg

typedef g2o::BlockSolver< g2o::BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic> > MyBlockSolver; typedef g2o::LinearSolverDense<MyBlockSolver::PoseMatrixType> MyLinearSolver; MyLinearSolver* linearSolver = new MyLinearSolver);//定义一个线性求解器 MyBlockSolver* solver_ptr = new MyBlockSolverlinearSolver);//定义一个块求解器 g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenbergsolver_ptr);//定义优化方法 optimizer.setAlgorithmsolver);//设置图优化的求解器

这里是自下而上的设置。
块求解器这块还没看懂，

之后添加顶点和边。

最后优化

optimizer.initializeOptimization); optimizer.setVerboseverbose); optimizer.optimizemaxIterations);