视觉SLAM十四讲学习笔记-第六讲-非线性优化的实践-高斯牛顿法和曲线拟合

专栏系列文章如下：

视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第二讲-开发环境搭建_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第三讲-旋转矩阵和Eigen库_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第三讲-旋转向量、欧拉角、四元数_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第三讲-相似、仿射、射影变换和eigen程序、可视化演示_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第四讲-李群与李代数基础和定义、指数和对数映射_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第四讲-李代数求导与扰动模型_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第四讲-Sophus实践、相似变换群与李代数_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第五讲-相机模型_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第五讲-图像和实践_goldqiu的博客-CSDN博客_global shutter

视觉SLAM十四讲学习笔记-第六讲-非线性优化的状态估计问题_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第六讲-非线性优化的非线性最小二乘问题_goldqiu的博客-CSDN博客

6.3 实践：曲线拟合问题

6.3.1手写高斯牛顿法

考虑一条满足以下方程的曲线：

其中 a,b,c 为曲线的参数，w 为高斯噪声，满足 w ∼ (0,σ2)。假设有 N 个关于 x,y 的观测数据点，根据这些数据点求出曲线的参数。那么，可以求解下面的最小二乘问题来估计曲线参数：

在这个问题中，待估计的变量是 a,b,c，而不是 x。程序里先根据模型生成 x,y 的真值，然后在真值中添加高斯分布的噪声。随后，使用高斯牛顿法来从带噪声的数据（x，y）拟合参数模型。定义误差为:

那么可以求出每个误差项对于状态变量的导数：

于是

高斯牛顿法的增量方程为：

也可以选择把所有的Ji 排成一列，将这个方程写成矩阵形式，它的含义与求和形式是一致的。下面的代码演示了这个过程是如何进行的：slambook2/ch6/gaussNewton.cpp

在这个例子中演示了如何对一个简单的拟合问题进行迭代优化。该程序输出每一步迭代的目标函数值和更新量，如下：

整个问题的目标函数在迭代 9 次之后趋近收敛，更新量趋近于零。最终估计的值与真值接近，函数图像如下：

蓝色点为100个数据点，黑色线为理论模型，红色线为拟合的模型。

6.3.2使用 Ceres 进行曲线拟合

Ceres 简介

Google Ceres 是一个广泛使用的最小二乘问题求解库。在 Ceres 中，只需按照一定步骤定义待解的优化问题，然后交给求解器计算即可。Ceres 求解的最小二乘问题最一般的形式如下（带边界的核函数最小二乘）：

在这个问题中，x1,··· ,xn 为优化变量，又称参数块（Parameter blocks），fi 称为代价函数（Cost function），也称为残差块（Residual blocks），在 SLAM 中也可理解为误差项。lj 和 uj 为第 j 个优化变量的上限和下限。在最简单的情况下，取 lj = −∞,uj = ∞（不限制优化变量的边界）。此时，目标函数由许多平方项经过一个核函数 ρ(·) 之后求和组成。取 ρ 为恒等函数，那么目标函数即为许多项的平方和，就得到了无约束的最小二乘问题。为了让 Ceres 求解这个问题，需要做以下几件事：

定义每个参数块。参数块通常为向量，但是在SLAM里也可以定义成四元数、李代数这种特殊的结构。如果是向量，那么需要为每个参数块分配一个 double 数组，来存储变量的值。
然后定义残差块的计算方式。残差块通常关联若干个参数块，对它们进行一些自定义的计算，然后返回残差值。Ceres 对它们求平方和之后，作为目标函数的值。
残差块往往也需要定义雅可比的计算方式。在 Ceres 中，可以使用自动求导功能，也可以手动指定雅可比的计算过程。如果要使用自动求导，那么残差块需要按照特定的写法来书写：残差的计算过程是一个带模板的括号运算符。
最后，把所有的参数块和残差块加入 Ceres 定义的 Problem 对象中，调用 Solve 函数求解即可。求解之前，可以传入一些配置信息，例如迭代次数、终止条件等，也可以使用默认的配置。

安装 Ceres

Ceres 的 github 地址为：https://github.com/ ceres-solver/ceres-solver，

安装依赖项

sudo apt−get install liblapack−dev libsuitesparse−dev libcxsparse3  libgflags−dev libgoogle−glog−dev libgtest−dev

然后进入 Ceres 库目录下，使用 cmake 编译并安装。安装完成后，在/usr/local/include/ceres 下找到 Ceres 的头文件，并在/usr/local/lib/下找到名为 libceres.a 的库文件。有了这些文件，就可以使用 Ceres 进行优化计算了。

使用 Ceres 拟合曲线

下面的代码演示了如何使用 Ceres 求解同样的问题：slambook/ch6/ceresCurveFitting.cpp

利用 OpenCV 的噪声生成器生成了 100 个带高斯噪声的数据，随后利用 Ceres 进行拟合。这里Ceres用法有如下几项：

定义残差块的类。方法是书写一个类（或结构体），并在类中定义带模板参数的 () 运算符，这样该类就成为了一个拟函数（Functor），或者叫仿函数。这种定义方式使得 Ceres 可以像调用函数一样，对该类的某个对象（比如 a）调用 a<double>() 方法。Ceres 会把雅可比矩阵作为类型参数传入此函数，从而实现自动求导的功能。
程序中的 double abc[3] 即为参数块，而对于残差块，对每一个数据构造 CURVE_FIT-TING_COST 对象，然后调用 AddResidualBlock 将误差项添加到目标函数中。由于优化需要梯度，有若干种选择：（1）使用 Ceres 的自动求导（Auto Diff）；（2）使用数值求导（Numeric Diff）；（3）自行推导解析的导数形式，提供给 Ceres。
自动求导需要指定误差项和优化变量的维度。这里的误差是标量，维度为 1；优化的是 a,b,c 三个量，维度为 3。于是，在自动求导类 AutoDiffCostFunction 的模板参数中设定变量维度为 1、3。
设定好问题后，调用 Solve 函数进行求解。可以在options里配置优化选项。例如，可以选择使用 Line Search 还是 Trust Region、迭代次数、步长，等等。可以查看 Options 的定义，看看有哪些优化方法可选，一般默认的配置已经可用于很广泛的问题了。

最终的优化值和手写的基本相同，但运行速度上 Ceres 要相对慢一些。

Ceres的优点是提供了自动求导工具，不必去计算很麻烦的雅可比矩阵。Ceres的自动求导是通过模板元实现的，在编译时期就可以完成自动求导工作，不过仍然是数值导数。此外，Ceres的优化过程配置也很丰富，使其适合很广泛的最小二乘优化问题，包括 SLAM 之外的各种问题。

注：自动求导也是用数值导数实现的。

6.3.3使用 g2o 进行曲线拟合

g2o（General Graphic Optimization，G2O）是在SLAM领域广为使用的优化库。它是一个基于图优化的库。图优化是一种将非线性优化与图论结合起来的理论。

图优化理论简介

前面介绍了非线性最小二乘的求解方式。它们是由很多个误差项之和组成的。然而，仅有一组优化变量和许多个误差项，并不清楚它们之间的关联。比如，某个优化变量xj存在于多少个误差项中呢？能保证对它的优化是有意义的吗？进一步，希望能够直观地看到该优化问题长什么样。于是，就引出了图优化。

图优化，是把优化问题表现成图（Graph）的一种方式。这里的图是图论意义上的图。一个图由若干个顶点（Vertex），以及连接着这些顶点的边（Edge）组成。进而，用顶点表示优化变量，用边表示误差项。于是，对任意一个上述形式的非线性最小二乘问题，可以构建与之对应的一个图。可以简单地称它为图，也可以用概率图里的定义，称之为贝叶斯图或因子图。

如下图：

用三角形表示相机位姿节点，用圆形表示路标点，它们构成了图优化的顶点；同时，实线表示相机的运动模型，虚线表示观测模型，它们构成了图优化的边。最基本的图优化是用图模型来表达一个非线性最小二乘的优化问题，可以利用图模型的某些性质做更好的优化。

g2o 是一个通用的图优化库，可以在g2o里求解任何能够表示为图优化的最小二乘问题，包括曲线拟合问题。

g2o 的编译与安装

安装依赖：

sudo apt−get install qt5−qmake qt5−default libqglviewer−dev−qt5 libsuitesparse−dev libcxsparse3 libcholmod3

从GitHub下载并cmake安装：https://github.com/RainerKuemmerle/g2o

安装完成后， g2o 的头文件将位于/usr/local/g2o 下，库文件位于/usr/local/lib/下

使用 g2o 拟合曲线

首先要将曲线拟合问题抽象成图优化。这个过程中，节点为优化变量，边为误差项。

在曲线拟合问题中，整个问题只有一个顶点：曲线模型的参数 a, b, c；而各个带噪声的数据点，构成了一个个误差项，也就是图优化的边。这里的边是一元边（Unary Edge），即只连接一个顶点。事实上，图优化中一条边可以连接一个、两个或多个顶点，这主要反映每个误差与多少个优化变量有关。

主要步骤：

定义顶点和边的类型。
构建图。
选择优化算法。
调用g2o进行优化，返回结果。

程序：slambook/ch6/g2oCurveFitting.cpp

在这个程序中，从g2o派生出了用于曲线拟合的图优化顶点和边：CurveFittingVertex 和 CurveFittingEdge，这实质上扩展了g2o的使用方式。这两个类分别派生于BaseVertex和BaseUnaryEdge类。在派生类中，重写了重要的虚函数：

顶点的更新函数：oplusImpl。优化过程最重要的是增量∆x的计算，而该函数处理的是 xk+1 = xk + ∆x 的过程。在曲线拟合过程中，由于优化变量（曲线参数）本身位于向量空间中，这个更新计算就是简单的加法。但是当优化变量不在向量空间中时，比如x是相机位姿，它本身不一定有加法运算。这时就需要重新定义增量如何加到现有的估计上的行为了。
顶点的重置函数：setToOriginImpl。即把估计值置零即可。
边的误差计算函数：computeError。该函数需要取出边所连接的顶点的当前估计值，根据曲线模型，与它的观测值进行比较。这和最小二乘问题中的误差模型是一致的。
边的雅可比计算函数：linearizeOplus。这个函数里计算了每条边相对于顶点的雅可比。
存盘和读盘函数：read、write。

定义了顶点和边之后，在main函数里声明了一个图模型，然后按照生成的噪声数据，往图模型中添加顶点和边，最后调用优化函数进行优化。g2o会给出优化的结果。

6.4 小结

非线性优化问题：由许多个误差项平方和组成的最小二乘问题。讨论了两种主要的梯度下降方式：高斯牛顿法和列文伯格 —马夸尔特方法。在实践部分中，分别使用了手写高斯牛顿法、Ceres 和 g2o 两种优化库求解同一个曲线拟合问题，发现结果相似。特别地，如果用g2o来拟合曲线，必须先把问题转换为图优化，定义新的顶点和边。相比之下，Ceres 定义误差项求曲线拟合问题则自然了很多，因为它本身即是一个优化库。然而，在 SLAM 中更多的问题是，一个带有许多个相机位姿和许多个空间点的优化问题如何求解。特别地，当相机位姿以李代数表示时，误差项关于相机位姿的导数如何计算。g2o提供了大量现成的顶点和边，非常便于相机位姿估计问题。而在 Ceres 中，不得不自己实现每一个Cost Function，有一些不便。

Ceres 库提供了基于模板元的自动求导和运行时的数值求导，而 g2o 只提供了运行时数值求导这一种方式。但是对于大多数问题，如果能够推导出雅可比矩阵的解析形式并告诉优化库，就可以避免数值求导中的诸多问题。

习题

证明线性方程 Ax = b 当系数矩阵 A 超定时，最小二乘解为 x = (ATA) −1ATb。
调研最速下降法、牛顿法、高斯牛顿法和列文伯格—马夸尔特方法各有什么优缺点。
为什么高斯牛顿法的增量方程系数矩阵可能不正定？不正定有什么几何含义？为什么在这种情况下解就不稳定了？
DogLeg 是什么？它与高斯牛顿法和列文伯格—马夸尔特方法有何异同？
阅读 Ceres 的教学材料（http://ceres-solver.org/tutorial.html）以更好地掌握其用法。
阅读 g2o 自带的文档。
更改曲线拟合实验中的曲线模型，并用 Ceres 和 g2o 进行优化实验。

习题在空余时间学习