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

一点儿也不萌的萌萌

已于 2023-09-05 11:38:42 修改

阅读量6.4k

点赞数 35

分类专栏： SLAM相关算法文章标签：人工智能算法机器学习数学非线性优化

于 2021-01-03 18:15:56 首次发布

本文链接：https://blog.csdn.net/u011341856/article/details/112134763

版权

SLAM相关算法专栏收录该内容

8 篇文章

订阅专栏

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

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

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

1.曲线拟合

在这里插入图片描述

图1

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

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

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

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

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

类似的，按照上面说的思路，要实现所有距离之和最小，可以用如下数学式来表达：
$\min_{a,b,\lambda} \sum_i^N (y_i-a\exp(-\lambda x_i + b)) \tag 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>()));
    //将优化算法设置给SparseOptimizer
    optimizer.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_NEW

    EdgePointOnCurve() = 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$的初始估计值都为1
    params->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