本文主要是介绍非线性优化库g2o使用教程,探索一些常见的用法,以及信息矩阵、鲁棒核函数对于优化的结果的影响,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!
本篇博客将总结一些常见的g2o用法。通过这篇内容你将至少可以大致掌握g2o的用法,以及一些可以使优化结果更好的小技巧,包括鲁邦和函数、信息矩阵的用法等等。
注意:本篇博客的重点是介绍g2o,所以不会去为非线性化方法做太多的铺垫,因此要想理解以下代码和思路,需要你具备一些非线性优化的理论知识,至少要明白什么是非线性优化,它主要是为了做什么,它是怎么实现的?
我们先来看第一个例子:曲线拟合
1.曲线拟合
我们现在有以下任务要求:找到一条函数曲线去拟合上图中的这些散点,使得所有点均匀的分散在这个拟合曲线的两侧。
散点:图一中那些离散的蓝色圆点。
这里我给出一种思路,主要是为了帮助对非线性优化不是很熟悉的同学。咱们想一下如果有这么一条曲线,所有散点到它的距离之和最小,那么是不是这条曲线就可以很好的拟合这些散点了。
下面我将通过一些数学公式来描述这个数学问题,但是我会省略一些过程。(请不要忘记我们目标是学习g2o的用法)
假设,我们要用来拟合这些散点的函数是: y = a exp ( − λ x + b ) y = a\exp(-\lambda x + b) y=aexp(−λx+b)
类似的,按照上面说的思路,要实现所有距离之和最小,可以用如下数学式来表达:
min a , b , λ ∑ i N ( y i − a exp ( − λ x i + b ) ) (1) \min_{a,b,\lambda} \sum_i^N (y_i-a\exp(-\lambda x_i + b)) \tag 1 a,b,λmini∑N(yi−aexp(−λxi+b))(1)
当然你也可以构造成的别的形式,方法并不唯一
我们的目标就是找到一组 a , b , λ a,b,\lambda a,b,λ的解,使得式(1)整体值最小,也就是各个点到曲线的距离在 y y y方向的和最小。
数学上处理(1)式的大致思路是:对其进行求导,然后通过导数确定函数值下降的方向,然后通过迭代的方式获得(1)式最小值时对应的 a , b , λ a,b,\lambda a,b,λ
不知道上面说的这些东西,你是否都理解,如果你觉得理解不了,你需要看一些关于非线性优化的资料,了解一些它的目的和思路!
下面我们就进入g2o优化的阶段,我们来看一下g2o是怎么处理这个问题的。在g2o中,对于优化问题统统都抽象成边和顶点来表示。
- 顶点:待优化的变量
- 边:每一个误差项
上述表述,有一些抽象。对应曲线拟合这个例子来,那么顶点就是我们要求的变量 a , b , λ a,b,\lambda a,b,λ,边就是每一个测量对应的误差,更具体一点儿来说就是 y i − a exp ( − λ x i + b ) y_i-a\exp(-\lambda x_i + b) yi−aexp(−λxi+b)的值。
那么这个曲线的拟合的例子中,就只有一个顶点,N条边!
只要是能把优化问题表示成顶点和边的形式,就可以非常容易的调用g2o来进行优化。
我们先来看一下g2o的类组成关系
我们从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,相差无几,对应的拟合曲线如下:
以上就是一个完整的g2o优化方法的使用流程。下面我们来做一些更细致的探讨!
鲁棒核函数
我们看一下这种情况,假设现在散点中一个很离谱的错误点,如图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使用教程,探索一些常见的用法,以及信息矩阵、鲁棒核函数对于优化的结果的影响的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!