非线性优化库g2o使用教程，探索一些常见的用法，以及信息矩阵、鲁棒核函数对于优化的结果的影响

本文主要是介绍非线性优化库g2o使用教程，探索一些常见的用法，以及信息矩阵、鲁棒核函数对于优化的结果的影响，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

本篇博客将总结一些常见的g2o用法。通过这篇内容你将至少可以大致掌握g2o的用法，以及一些可以使优化结果更好的小技巧，包括鲁邦和函数、信息矩阵的用法等等。

注意：本篇博客的重点是介绍g2o，所以不会去为非线性化方法做太多的铺垫，因此要想理解以下代码和思路，需要你具备一些非线性优化的理论知识，至少要明白什么是非线性优化，它主要是为了做什么，它是怎么实现的？

我们先来看第一个例子：曲线拟合

1.曲线拟合

图1

我们现在有以下任务要求：找到一条函数曲线去拟合上图中的这些散点，使得所有点均匀的分散在这个拟合曲线的两侧。

散点：图一中那些离散的蓝色圆点。

这里我给出一种思路，主要是为了帮助对非线性优化不是很熟悉的同学。咱们想一下如果有这么一条曲线，所有散点到它的距离之和最小，那么是不是这条曲线就可以很好的拟合这些散点了。

下面我将通过一些数学公式来描述这个数学问题，但是我会省略一些过程。（请不要忘记我们目标是学习g2o的用法）

假设，我们要用来拟合这些散点的函数是：$y=a\exp (-\lambda x+b)$

类似的，按照上面说的思路，要实现所有距离之和最小，可以用如下数学式来表达：
$$\underset{a,b,\lambda }{\min }\sum _i^N(y_i-a\exp (-\lambda x_i+b))\text{\hspace{1em}(1)}$$

当然你也可以构造成的别的形式，方法并不唯一

我们的目标就是找到一组$a,b,\lambda$的解，使得式（1）整体值最小，也就是各个点到曲线的距离在$y$方向的和最小。

数学上处理（1）式的大致思路是：对其进行求导，然后通过导数确定函数值下降的方向，然后通过迭代的方式获得（1）式最小值时对应的$a,b,\lambda$

不知道上面说的这些东西，你是否都理解，如果你觉得理解不了，你需要看一些关于非线性优化的资料，了解一些它的目的和思路！

下面我们就进入g2o优化的阶段，我们来看一下g2o是怎么处理这个问题的。在g2o中，对于优化问题统统都抽象成边和顶点来表示。

• 顶点：待优化的变量
• 边：每一个误差项

上述表述，有一些抽象。对应曲线拟合这个例子来，那么顶点就是我们要求的变量$a,b,\lambda$，边就是每一个测量对应的误差，更具体一点儿来说就是$y_i-a\exp (-\lambda x_i+b)$的值。

那么这个曲线的拟合的例子中，就只有一个顶点，N条边！

只要是能把优化问题表示成顶点和边的形式，就可以非常容易的调用g2o来进行优化。

我们先来看一下g2o的类组成关系

图2

我们从SparseOptimizer这个类开始看，它需要一个OptimazationAlgorithm，g2o中提供了三种优化算法可以选择，GN、LM、DogLeg。而OptimazationAlgorithm需要一个Solver，同样的可以有多种求解器来选择。类似的可以看到SparseOptimizer就是一个HyperGraph，它由多个边和多个顶点组成。

总结起来，g2o的用法就是先构造优化算法，然后构造边和顶点，最后就可以进行优化的操作了。

下面咱们先来构造优化优化算法，代码如下：

	//为了代码简洁typedef g2o::BlockSolver<g2o::BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic> > MyBlockSolver;//block求解器typedef g2o::LinearSolverDense<MyBlockSolver::PoseMatrixType> MyLinearSolver;//线性求解器// 初始化一个SparseOptimizer对象g2o::SparseOptimizer optimizer；//初始化一个优化算法g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(g2o::make_unique<MyBlockSolver>(g2o::make_unique<MyLinearSolver>()));//将优化算法设置给SparseOptimizeroptimizer.setAlgorithm(solver);

以上就是一个最简单的SparseOptimizer对象的构造方法，有了这个优化器，然后再添加边和顶点：
顶点

//根据图2的顶点构造关系，需要从基类中继承，然后对基类BaseVertex中的一些虚函数进行实现
class VertexParams : public g2o::BaseVertex<3, Eigen::Vector3d> {
public://Eigen自动内存对齐EIGEN_MAKE_ALIGNED_OPERATOR_NEW;VertexParams() = default;//默认构造函数bool read(std::istream & /*is*/) override {cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;return false;}bool write(std::ostream & /*os*/) const override {cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;return false;}void setToOriginImpl() override {cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;}//设置顶点估计值的更新void oplusImpl(const double *update) override {Eigen::Vector3d::ConstMapType v(update);_estimate += v;}
};

边

//按照图2的流程，需要从基类中继承，由于我们这里顶点只有一个，所以就选用一元边，
//那么就从一元边的基类BaseUnaryEdge中继承，然后重写其中的一些重要虚函数
class EdgePointOnCurve : public g2o::BaseUnaryEdge<1, Eigen::Vector2d, VertexParams> {
public://Eigen自动内存对齐EIGEN_MAKE_ALIGNED_OPERATOR_NEWEdgePointOnCurve() = default;//默认构造函数，比手动效率更高bool read(std::istream & /*is*/) override {cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;return false;}bool write(std::ostream & /*os*/) const override {cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;return false;}//	误差的计算函数void computeError() override {const VertexParams *params = dynamic_cast<const VertexParams *>(vertex(0));const double &a = params->estimate()(0);const double &b = params->estimate()(1);const double &lambda = params->estimate()(2);double fval = a * exp(-lambda * measurement()(0)) + b;_error(0) = std::abs(fval - measurement()(1));}
};

以上就定义完成了，曲线拟合任务优化的顶点和边

然后就需要将顶点和边添加到优化器中：

添加顶点

    VertexParams *params = new VertexParams();params->setId(0);//设置顶点编号// 设置顶点的初始估计值，相当于a, b, $\lambda$的初始估计值都为1params->setEstimate(Eigen::Vector3d(1, 1, 1)); optimizer.addVertex(params);//将顶点添加到优化器中

添加边

for (int i = 0; i < numPoints; ++i) {//新建一个边EdgePointOnCurve *e = new EdgePointOnCurve;e->setInformation(Eigen::Matrix<double, 1, 1>::Identity());//信息矩阵e->setVertex(0, params);//设置边对应的顶点e->setMeasurement(points[i]);//设置边的测量值optimizer.addEdge(e);}

然后就可以进行优化了，对应的代码如下：

    optimizer.initializeOptimization();//初始化整个优化器optimizer.optimize(maxIterations);//开始执行优化，迭代的次数为maxIterations//输出最终优化得到的结果
cout << params->estimate()[0] << ", "<< params->estimate()[1] << ", "<< params->estimate()[2] << endl;

1.98896, 0.406936, 0.201035

该结果与我们设置的真值：2，0.4，0.2，相差无几，对应的拟合曲线如下：

图3

以上就是一个完整的g2o优化方法的使用流程。下面我们来做一些更细致的探讨！

鲁棒核函数

我们看一下这种情况，假设现在散点中一个很离谱的错误点，如图4

图4

由于右上角那个离谱的点，导致优化时将整个函数被拉偏了（可以对比图3）。

那么怎么解决这种问题呢？g2o中提供了鲁棒核函数来抑制某些误差特别大的点，拉偏整个优化结果。

鲁棒核函数不是g2o独有的，这是非线性优化方法中的一种常用手段！

使用方法如下：

		//构造一个Huber鲁棒核函数g2o::RobustKernelHuber* robust_kernel_huber = new g2o::RobustKernelHuber;robust_kernel_huber->setDelta(0.3);//设置delta的大小。注意这个要根据实际的应用场景去尝试，然后选择合适的大小e->setRobustKernel(robust_kernel_huber);//向边中添加鲁棒核函数

g2o中提供了多种鲁棒核函数，你可以根据自己的需要进行选择。

加入鲁棒核函数之后，结果明显好转。

如果你不了解鲁棒核函数的作用，你需要查看一下资料去学习一下

信息矩阵
现在来考虑另一种情况，比方说在一次优化中，对于某一次测量，我们有十足的把握，它非常的准确，所以优化时我们希望对于这次测量给予更高的权重。

如上图，假设我们认为左上角那个异常点是一个比较正确的点（只是假设），我们希望拟合的曲线尽量往这个点偏移。那么我们就这可以设置这次测量边的权重更大。

代码如下：

e->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 10);

因为测量值的维度为1，所以信息矩阵也为1。如果我们把每一条边的信息矩阵都设置为一样，那么在优化时将认为所有边的优化权重是一样的，将不会对某一条边执行过多的优化！

对于那个异常点设置权重为别的点的10倍，则曲线会往右上角那个点靠。最终的结果如下图：

一般情况下，信息矩阵和鲁棒核函数都会一起使用！

完整代码

如果你觉得上面代码中很多细节难以理解,那你不必花太多时间去理解细节,先从整体上去理解g2o的用法,然后多尝试一些例子,你的疑惑就会迎刃而解了!

2.更复杂的应用

TODO

这篇关于非线性优化库g2o使用教程，探索一些常见的用法，以及信息矩阵、鲁棒核函数对于优化的结果的影响的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！

---