slam入门——十四讲笔记（五）_slam中的先验误差

第六讲 非线性优化

【本节目标】

1. 理解最小二乘法的含义和处理方式
2. 理解Gauss-Newton，Levenburg-Marquadt等下降策略
3. 学习Ceres库和g2o库的基本使用方法

在前面几章，我们介绍了经典SLAM模型的运动方程和观测方程。现在我们已经知道，方程中的位姿可以由变换矩阵来描述，然后用李代数进行优化。观测方程由相机成像模型给出，其中内参是随相机固定的，而外参是相机的位姿。于是，我们已经弄清了经典SLAM模型在视觉情况下的具体表达。

然而，由于噪声的存在，运动方程和观测方程的等式必定不是精确成立的。尽管相机可以非常好地符合针孔模型，但遗憾的是，我们得到的数据通常是受各种未知噪声影响的。即使我们有着高精度的相机，运动方程和观测方程也只能近似的成立。所以，与其假设数据必须符合方程，不如来讨论，如何在有噪声的数据中进行准确的状态估计。

大多现代视觉 SLAM 算法都不需要那么高成本的传感器，甚至也不需要那么昂贵的处理器来计算这些数据，这全是算法的功劳。由于在 SLAM 问题中，同一个点往往会被一个相机在不同的时间内多次观测，同一个相机在每个时刻观测到的点也不止一个。这些因素交织在一起，使我们拥有了更多的约束，最终能够较好地从噪声数据中恢复出我们需要的东西。本节就将介绍如何通过优化处理噪声数据，并且由这些表层逐渐深入到图优化本质，提供图优化的解决算法初步介绍并且提供训练实例。

6.1 状态估计问题

6.1.1 最大后验与最大似然

接着前面几章的内容，我们回顾一下第二讲讨论的经典SLAM模型。它由一个状态方程和一个运动方程构成，如式（2.5）所示：
{ x k = f ( x k − 1 , u k ) + w k z k , j = h ( y j , x k ) + v k , j ( 6.1 ) \left\{

$$\begin{aligned} x_k &= f(x_{k-1},u_k) + w_k \\ z_{k,j} &= h(y_j, x_k) +v_{k,j} \end{aligned}$$ \right. \qquad (6.1) {xk​zk,j​​=f(xk−1​,uk​)+wk​=h(yj​,xk​)+vk,j​​(6.1)
通过第四讲的知识，我们了解到这里的$x_k$是相机的位姿。我们可以使用变换矩阵或者李代数表示它。至于观测方程，第五讲已经说明了它的内容，即针孔相机模型。为了让读者对它们有更深的印象，我们不妨讨论一下他们的具体参数化形式。首先，位姿变量$x_k$可以由$T_k$或$exp({\xi }_k^{\wedge })$表达，二者是等价的。由于运动方程在视觉SLAM中没有特殊性，我们暂且不讨论它，主要讨论观测方程。假设在$x_k$处对路标$y_j$进行了一次观测，对应到图像上的像素位置$z_{k,j}$，那么，观测方程可以表示成：
$$s{z}_{k,j}=Kexp({\xi }^{\wedge })y_j(6.2)$$
根据上一讲的内容，我们应该知道这里$K$为相机内参，s为像素点的距离。同时这里的$z_{k,j}$和$y_j$都必须以齐次坐标来描述，且中间有一次齐次到非齐次的转换。

现在，考虑数据受噪声的影响后，会发生什么改变。在运动和观测方程中，我们通常假设两个噪声项$w_k,v_{k,j}$满足零均值的高斯分布：
$$w_k\sim N(0,R_k),v_k\sim N(0,Q_{k,j})(6.3)$$
在这些噪声的影响下，我们希望通过带噪声的数据$z$和$u$，推断位姿$x$和地图$y$（以及它们的概率分布），这构成了一个状态估计问题。由于在SLAM过程中，这些数据是随着时间逐渐到来的，所以在历史上很长一段时间内，研究者们使用滤波器，尤其是扩展卡尔曼滤波器（EKF）求解它。卡尔曼滤波器关心当前时刻的状态估计$x_k$，而对之前的状态则不多考虑；相对的，今年来普遍使用的非线性优化方法，使用所有时刻采集到的数据进行状态估计，并被认为优于传统的滤波器，成为当前视觉SLAM的主流方法。因此，本书重点介绍以非线性优化为主的优化方法，对卡尔曼滤波器则留到第十讲再进行讨论。本讲将介绍非线性优化的基本知识，然后在第十、十一讲中对它们进行更深入的分析。

首先，我们从概率学角度看一下我们正在讨论什么问题。在非线性优化中，我们把所有待估计的变量放在一个“状态变量”中：
x = { x 1 , ⋯   , x N , y 1 , ⋯   , y M } . x = \left\{x_1, \cdots, x_N, y_1, \cdots, y_M \right\}. x={x1​,⋯,xN​,y1​,⋯,yM​}.
现在，我们说，对机器人状态的估计，就是求已知输入数据$u$和观测数据$z$的条件下，计算状态$x$的条件概率分布：
$$P(x|z,u).(6.4)$$
类似于$x$,这里的$u$和$z$也是对所有数据的统称。特别地，当我们没有测量运动的传感器，只有一张张的图像时，即只考虑观测方程带来的数据时，相当于估计$P(x|z)$的条件概率分布。

如果忽略图像在时间上的联系，把他们看作一堆彼此没有关系的图片，该问题也称为Structure from Motion(SfM)，即如何从许多图像中重建三维空间结构。在这种情况下，SLAM可以看作是图像具有时间先后顺序的，需要实时求解一个SfM问题。为了估计状态变量的条件分布，利用贝叶斯法则，有：
$$P(x|z)=\frac{P(z|x)P(x)}{P(z)}\propto P(z|x)P(x)(6.5)$$
贝叶斯法则左侧通常称为后验概率。它右侧的P(z|x)称为似然，另一部分P(x)称为先验。直接求后验分布是困难的，但是求一个状态最优估计，使得在该状态下，后验概率最大化（Maximize a Posterior, MAP），则是可行的：
$$x_{MAP}^{\ast }=\mathrm{argmax}P(x|z)=\mathrm{argmax}P(z|x)P(x).(6.6)$$
请注意贝叶斯法则的分母部分与待估计的状态 $x$ 无关，因而可以忽略。贝叶斯法则告 诉我们，求解最大后验概率，相当于最大化似然和先验的乘积。进一步，我们当然也可以 说，对不起，我不知道机器人位姿大概在什么地方，此时就没有了先验。那么，可以求解 $x$ 的最大似然估计（Maximize Likelihood Estimation, MLE）：
$$x_{MLE}^{\ast }=\mathrm{argmax}P(z|x).(6.7)$$
直观地说，似然是指“在现在的位姿下，可能产生怎样的观测数据”。由于我们知道观测数据，所以最大似然估计，可以理解成：“在什么样的状态下，最可能产生现在观测到的数据”。这就是最大似然估计的意义。

6.1.2 最小二乘的引出

那么如何求最大似然估计呢？我们说，在高斯分布的假设下，最大似然能够有较简单 的形式。回顾观测模型，对于某一次观测：
$$z_{k,j}=h(y_j,x_k)+v_{k,j},$$
由于我们假设了噪声项$v_k\sim N(0,Q_{k,j})$，所以观测数据的条件概率为：
$$P(z_{j,k}|x_k,y_j)=N(h(y_j,x_k),Q_{k,j}).$$
它依然是一个高斯分布。为了计算使它最大化的 $x_k,y_j$，我们往往使用最小化负对数的方式，来求一个高斯分布的最大似然。 高斯分布在负对数下有较好的数学形式。考虑一个任意的高维高斯分布 $x\sim N(\mu ,\Sigma )$， 它的概率密度函数展开形式为:


这就得到了一个总体意义下的最小二乘问题（Least Square Problem）。我们明白它的最优 解等价于状态的最大似然估计。直观来讲，由于噪声的存在，当我们把估计的轨迹与地图 代入 SLAM 的运动、观测方程中时，它们并不会完美的成立。这时候怎么办呢？我们把状 态的估计值进行微调，使得整体的误差下降一些。当然这个下降也有限度，它一般会到达一个极小值。这就是一个典型非线性优化的过程。
仔细观察式（6.12），我们发现 SLAM 中的最小二乘问题具有一些特定的结构：

• 首先，整个问题的目标函数由许多个误差的（加权的）平方和组成。虽然总体的状 态变量维数很高，但每个误差项都是简单的，仅与一两个状态变量有关。例如运动 误差只与 $x_{k-1},x_k$ 有关，观测误差只与 $x_k,y_j$ 有关。每个误差项是一个小规模的约 束，我们之后会谈论如何对它们进行线性近似，最后再把这个误差项的小雅可比矩阵 块放到整体的雅可比矩阵中。由于这种做法，我们称每个误差项对应的优化变量为参数块（Parameter Block）。
• 整体误差由很多小型误差项之和组成的问题，其增量方程的求解会具有一定的稀疏 性（会在第十讲详细讲解），使得它们在大规模时亦可求解。
• 其次，如果使用李代数表示，则该问题是无约束的最小二乘问题。但如果用旋转矩阵 （变换矩阵）描述位姿，则会引入旋转矩阵自身的约束（旋转矩阵必须是正交阵且行 列式为 1）。额外的约束会使优化变得更困难。这体现了李代数的优势。
• 最后，我们使用了平方形式（二范数）度量误差，它是直观的，相当于欧氏空间中距离的平方。但它也存在着一些问题，并且不是唯一的度量方式。我们亦可使用其他的范数构建优化问题。

现在，我们要介绍如何求解这个最小二乘问题。本章将介绍非线性优化的基本知识，特别地，针对这样一个通用的无约束非线性最小二乘问题，探讨它是如何求解的。在后续几章，我们会大量使用本章的结果，详细讨论它在SLAM前端、后端中的应用。

6.2 非线性最小二乘

我们先来考虑一个简单的最小二乘问题：
$$\underset{x}{\min }\frac{1}{2}||f(x)|{|}_2^2.(6.13)$$
这里自变量$x\in {\mathbb{R}}^n,f$是任意一个非线性函数，我们设它有$m$维：$f(x)\in {\mathbb{R}}^m$。下面讨论如何求解这样一个优化问题。

如果$f$是个数学形式上很简单的函数，那问题也许可以用解析形式来求。令目标函数的导数为零，然后求解$x$的最优值，就和一个求二元函数的极值一样：
$$\frac{df}{dx}=0.(6.14)$$
解此方程，就得到了导数为零处的极值。它们可能是极大、极小或鞍点处的值，只要挨 个儿比较它们的函数值大小即可。但是，这个方程是否容易求解呢？这取决于$f$导函数的 形式。在 SLAM 中，我们使用李代数来表示机器人的旋转和位移。尽管我们在李代数章节讨论了它的导数形式，但这不代表我们就能够顺利求解上式这样一个复杂的非线性方程。对于不方便直接求解的最小二乘问题，我们可以用迭代的方式，从一个初始值出发，不断地更新当前的优化变量，使目标函数下降。具体步骤可列写如下：

这让求解导函数为零的问题，变成了一个不断寻找梯度并下降的过程。直到某个时刻 增量非常小，无法再使函数下降。此时算法收敛，目标达到了一个极小，我们完成了寻找极小值的过程。在这个过程中，我们只要找到迭代点的梯度方向即可，而无需寻找全局导 函数为零的情况。

接下来的问题是，增量 $\Delta x_k$如何确定？——实际上，研究者们已经花费了大量精力探 索增量的求解方式。我们将介绍两类办法，它们用不同的手段来寻找这个增量。目前这两种方法在视觉 SLAM 的优化问题上也被广泛采用，大多数优化库都可以使用它们。

6.2.1 一阶和二阶梯度法

求解增量最直观的方式是将目标函数在$x$附近进行泰勒展开：
$$||f(x+\Delta x)|{|}_2^2\approx ||f(x)|{|}_2^2+J(x)\Delta x+\frac{1}{2}\Delta x^{T}H\Delta x.(6.15)$$
这里$J$是$||f(x)|{|}^2关于$x$的导数（雅可比矩阵），而$H$则是二阶导数（海塞（Hessian）矩阵）。我们可以选择保留泰勒展开的一阶或二阶项，对应的求解方法则为一阶梯度或二阶梯度法。如果保留一阶梯度，那么增量的方向为：
$$\Delta x^{\ast }=-J^T(x).(6.16)$$
它的直观意义非常简单，只要我们沿着反向梯度方向前进即可。当然，我们还需要该方向上取一个步长 $\lambda$，求得最快的下降方式。这种方法被称为最速下降法。

另一方面，如果保留二阶梯度信息，那么增量方程为：
$$\Delta x^{\ast }=\mathrm{argmin}||f(x)|{|}_2^2+J(x)\Delta x+\frac{1}{2}\Delta x^{T}H\Delta x.(6.17)$$
求右侧等式关于$\Delta x$的导数并令它为零，就得到了增量的解：
$$H\Delta x=-J^T.(6.18)$$
该方法称又为牛顿法。我们看到，一阶和二阶梯度法都十分直观，只要把函数在迭代点附近进行泰勒展开，并针对更新量作最小化即可。由于泰勒展开之后函数变成了多项式，所以求解增量时只需解线性方程即可，避免了直接求导函数为零这样的非线性方程的困难。

不过，这两种方法也存在它们自身的问题。最速下降法过于贪心，容易走出锯齿路线，反而增加了迭代次数。而牛顿法则需要计算目标函数的 $H$ 矩阵，这在问题规模较大时非常困难，我们通常倾向于避免 $H$ 的计算。所以，接下来我们详细地介绍两类更加实用的方法：高斯牛顿法和列文伯格——马夸尔特方法。

6.2.2 Gauss-Newton

Gauss Newton是最优化算法里面最简单的方法之一。它的思想是将$f(x)$进行一阶的泰勒展开（请注意不是目标函数$f(x{)}^2$）：

这个方程与之前有什么不一样呢？根据极值条件，将上述目标函数对$\Delta x$求导，并令导数为零。由于这里考虑的是$\Delta x$的导数（而不是 $x$），我们最后将得到一个线性的方程。为此，先展开目标函数的平方项：

从算法步骤中可以看到，增量方程的求解占据着主要地位。原则上，它要求我们所用 的近似 $H$ 矩阵是可逆的（而且是正定的），但实际数据中计算得到的 $J^{T}J$ 却只有半正定性。也就是说，在使用 Gauss Newton 方法时，可能出现 $J^{T}J$ 为奇异矩阵或者病态 (illcondition) 的情况，此时增量的稳定性较差，导致算法不收敛。更严重的是，就算我们假设 $H$ 非奇异也非病态，如果我们求出来的步长 $∆x$ 太大，也会导致我们采用的局部近似 (6.19) 不够准确，这样一来我们甚至都无法保证它的迭代收敛，哪怕是让目标函数变得更大都是有可能的。

尽管 Gauss Newton 法有这些缺点，但是它依然值得我们去学习，因为在非线性优化里，相当多的算法都可以归结为 Gauss Newton 法的变种。这些算法都借助了 Gauss Newton 法的思想并且通过自己的改进修正 Gauss Newton 法的缺点。例如一些线搜索方法 (line search method)，这类改进就是加入了一个标量 $\alpha$，在确定了 $∆x$ 进一步找到 $\alpha$ 使得 $\Vert f(x+\alpha ∆x){\Vert }^2$ 达到最小，而不是像 Gauss Newton 法那样简单地令 $\alpha =1$。

Levenberg-Marquadt 方法在一定程度上修正了这些问题，一般认为它比 Gauss Newton 更为鲁棒。尽管它的收敛速度可能会比 Gauss Newton 更慢，被称之为阻尼牛顿法 (Damped Newton Method)，但是在 SLAM 里面却被大量应用。

6.2.3 Levenberg-Marquadt

由于 Gauss-Newton 方法中采用的近似二阶泰勒展开只能在展开点附近有较好的近似效果，所以我们很自然地想到应该给 $∆x$ 添加一个信赖区域（Trust Region），不能让它太大而使得近似不准确。非线性优化种有一系列这类方法，这类方法也被称之为信赖区域方法 (Trust Region Method)。在信赖区域里边，我们认为近似是有效的；出了这个区域，近似可能会出问题。

那么如何确定这个信赖区域的范围呢？一个比较好的方法是根据我们的近似模型跟实际函数之间的差异来确定这个范围：如果差异小，我们就让范围尽可能大；如果差异大，我们就缩小这个近似范围。因此，考虑使用
$$\rho =\frac{f(x+\Delta x)-f(x)}{J(x)\Delta x}.(6.23)$$
来判断泰勒近似是否够好。$\rho$ 的分子是实际函数下降的值，分母是近似模型下降的值。如果 $\rho$ 接近于 $1$，则近似是好的。如果 $\rho$ 太小，说明实际减小的值远少于近似减小的值，则认为近似比较差，需要缩小近似范围。反之，如果 $\rho$ 比较大，则说明实际下降的比预计的更大，我们可以放大近似范围。

于是，我们构建一个改良版的非线性优化框架，该框架会比 Gauss Newton 有更好的效果：

这里近似范围扩大的倍数和阈值都是经验值，可以替换成别的数值。在式（6.24）中， 我们把增量限定于一个半径为 $\mu$ 的球中，认为只在这个球内才是有效的。带上$D$ 之后，这 个球可以看成一个椭球。在 Levenberg 提出的优化方法中，把 $D$ 取成单位阵 $I$，相当于 直接把 $∆x$ 约束在一个球中。随后，Marqaurdt 提出将 $D$ 取成非负数对角阵——实际中 通常用 $J^{T}J$ 的对角元素平方根，使得在梯度小的维度上约束范围更大一些。

不论如何，在 L-M 优化中，我们都需要解式（6.24）那样一个子问题来获得梯度。这个子问题是带不等式约束的优化问题，我们用 Lagrange 乘子将它转化为一个无约束优化问题：


我们看到，当参数 $\lambda$ 比较小时，$H$ 占主要地位，这说明二次近似模型在该范围内是比较好的，L-M 方法更接近于 G-N 法。另一方面，当 $\lambda$ 比较大时，$\lambda I$ 占据主要地位，L-M 更接近于一阶梯度下降法（即最速下降），这说明附近的二次近似不够好。L-M 的求解方 式，可在一定程度上避免线性方程组的系数矩阵的非奇异和病态问题，提供更稳定更准确的增量 $∆x$。

在实际中，还存在许多其它的方式来求解函数的增量，例如 Dog-Leg 等方法。我们在这里所介绍的，只是最常见而且最基本的方式，也是视觉 SLAM 中用的最多的方式。

总而言之，非线性优化问题的框架，分为 Line Search 和 Trust Region 两类。

• Line Search 先固 定搜索方向，然后在该方向寻找步长，以最速下降法和 Gauss-Newton 法为代表。
• 而 Trust Region 则先固定搜索区域，再考虑找该区域内的最优点。此类方法以 L-M 为代表。实际问题中，我们通常选择 G-N 或 L-M 之一作为梯度下降策略。

6.2.4 小结

由于作者不希望这本书变成一本让人觉得头疼的数学书，所以这里只罗列了最常见的两种非线性优化方案，Gauss Newton 和 Levernberg-Marquardt。我们避开了许多数学性质 上的讨论。如果读者对优化感兴趣，可以进一步阅读专门介绍数值优化的书籍（这是一个很大的课题）。以 G-N 和 L-M 为代表的优化方法，在很多开源的优化库都已经实现并提供给用户，我们会在下文进行实验。最优化是处理许多实际问题的基本数学工具，不光在视觉 SLAM 起着核心作用，在类似于深度学习等其它领域，它也是求解问题的核心方法之一。我们希望读者能够根据自身能力，去了解更多的最优化算法。

也许你发现了，无论是 G-N 还是 L-M，在做最优化计算的时候，都需要提供变量的初始值。你也许会问到，这个初始值能否随意设置? 当然不是。实际上非线性优化的所有迭代求解方案，都需要用户来提供一个良好的初始值。由于目标函数太复杂，导致在求解空间上的变化难以琢磨，对问题提供不同的初始值往往会导致不同的计算结果。这种情况是非线性优化的通病：大多数算法都容易陷入局部极小值。因此，无论是哪类科学问题，我 们提供初始值都应该有科学依据，例如视觉 SLAM 问题中，我们会用 ICP，PnP 之类的 算法提供优化初始值。总之，一个良好的初始值对最优化问题非常重要！

也许读者还会对上面提到的最优化产生疑问：如何求解线性增量方程组呢？我们只讲到了增量方程是一个线性方程，但是直接对系数矩阵进行求逆岂不是要进行大量的计算？ 当然不是。在视觉 SLAM 算法里，经常遇到 $∆x$ 的维度大到好几百或者上千，如果你是要做大规模的视觉三维重建，就会经常发现这个维度可以轻易达到几十万甚至更高的级别。要对那么大个矩阵进行求逆是大多数处理器无法负担的，因此存在着许多针对线性方程组 的数值求解方法。在不同的领域有不同的求解方式，但几乎没有一种方式是直接求系数矩阵的逆，我们会采用矩阵分解的方法来解线性方程，例如 QR、Cholesky 等分解方法。这些方法通常在矩阵论等教科书中可以找到，我们不多加介绍。

幸运的是，视觉 SLAM 里，这个矩阵往往有特定的稀疏形式，这为实时求解优化问题提供了可能性。我们在第十章中详细介绍它的原理。利用稀疏形式的消元，分解，最后再进 行求解增量，会让求解的效率大大提高。在很多开源的优化库上，维度为一万多的变量在一般的 PC 上就可以在几秒甚至更短的时间内就被求解出来，其原因也是因为用了更加高级的数学工具。视觉 SLAM 算法现在能够实时地实现，也是多亏了这系数矩阵是稀疏的，如果是矩阵是稠密的，恐怕优化这类视觉 SLAM 算法就不会被学界广泛采纳了。

6.3 实践：Ceres

我们前面说了很多理论，现在来实践一下前面提到的优化算法。在本章的实践部分中，
我们主要向大家介绍两个 C++ 的优化库：来自谷歌的 Ceres 库以及基于图优化的 g2o 库。由于 g2o 的使用还需要讲一点图优化的相关知识，所以我们先来介绍 Ceres， 然后介绍一些图优化理论，最后来讲g2o。由于优化算法在之后的视觉里程计和后端中都会出现，所以请读者务必掌握优化算法的意义，理解程序的内容。

6.3.1 Ceres简介

Ceres 库面向通用的最小二乘问题的求解，作为用户，我们需要做的就是定义优化问题，然后设置一些选项，输入进 Ceres 求解即可。Ceres 求解的最小二乘问题最一般的形式如下（带边界的核函数最小二乘）：

可以看到，目标函数由许多平方项，经过一个核函数 $\rho (\cdot )$ 之后，求和组成。在最简单的情况下，取 $\rho$ 为恒等函数，则目标函数即为许多项的平方和。在这个问题中，优化变量为 $x_1,...,x_n，f_i$ 称为代价函数（Cost function），在 SLAM 中亦可理解为误差项。$l_j$ 和 $u_j$ 为第 $j$ 个优化变量的上限和下限。在最简单的情况下，取 $l_j=-\infty ,u_j=\infty$（不限 制优化变量的边界），并且取 $\rho$ 为恒等函数时，就得到了无约束的最小二乘问题，和我们先前说的是一致的。 在 Ceres 中，我们将定义优化变量 $x$ 和每个代价函数 $f_i$，再调用 Ceres 进行求解。我们可以选择使用 G-N 或者 L-M 进行梯度下降，并设定梯度下降的条件，Ceres 会在优化 之后，将最优估计值返回给我们。下面，我们通过一个曲线拟合的实验，来实际操作一下 Ceres，理解优化的过程。

6.3.2 安装Ceres

去 github 上下载 Ceres：https://github.com/ceres-solver/ ceres-solver。本书的 3rdparty 下也附带了 Ceres 库。 与之前碰到的库一样，Ceres 是一个 cmake 工程。先来安装它的依赖项，在 Ubuntu 中都可以用 apt-get 安装，主要是谷歌自己使用的一些日志和测试工具：

sudo apt-get install liblapack-dev libsuitesparse-dev libcxsparse3.1.2 libgflags-dev libgoogle-glog-dev libgtest-dev
1

然后，进入 Ceres 库，使用 cmake 编译并安装它。这个过程我们已经做过很多遍了，此处就不再赘述。安装完成后，在/usr/local/include/ceres 下找到 Ceres 的头文件，并 在/usr/local/lib/下找到名为 libceres.a 的库文件。有了头文件和库文件，就可以使用 Ceres 进行优化计算了。

6.3.3 使用Ceres拟合曲线

我们的演示实验包括使用 Ceres 和接下来的 g2o 进行曲线拟合。假设有一条满足以下方程的曲线：
$$y=exp(a{x}^2+bx+c)+w,$$
其中 $a,b,c$ 为曲线的参数，$w$ 为高斯噪声。我们故意选择了这样一个非线性模型,以使问题不至于太简单。现在，假设我们有 $N$ 个关于 $x,y$ 的观测数据点，想根据这些数据点求出曲线的参数。那么，可以求解下面的最小二乘问题以估计曲线参数：
$$\underset{a,b,c}{\min }\frac{1}{2}\sum _{i=1}^N||y_i-exp(a{x}_i^2+b{x}_i+c)|{|}^2.(6.28)$$
请注意，在这个问题中，待估计的变量是 $a,b,c$，而不是 $x$。我们写一个程序，先根据模型生成 $x,y$ 的真值，然后在真值中添加高斯分布的噪声。随后，使用 Ceres 从带噪声的数据中拟合参数模型。

#include <iostream>
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>
#include <chrono>

using namespace std;

// 代价函数的计算模型
struct CURVE_FITTING_COST {
    CURVE_FITTING_COST (double x, double y) : _x ( x ), _y ( y ) {}

    // 残差的计算
    template <typename T>
    bool operator() (
            const T* const abc, // 模型参数，有3维
            T* residual ) const  // 残差
    {
        residual[0] = T(_y) - ceres::exp(abc[0]*T (_x) * T(_x) + abc[1]*T(_x) + abc[2]); // y-exp(ax^2+bx+c)
        return true;
    }

    const double _x, _y; // x, y 数据
};

int main() {
    double a=1.0, b=2.0, c=1.0;         // 真实参数值
    int N=100;                          // 数据点
    double w_sigma=1.0;                 // 噪声Sigma值
    cv::RNG rng;                        // OpenCV随机数产生器
    double abc[3] = {0,0,0};            // abc参数的估计值

    vector<double> x_data, y_data;      // 数据

    cout << "generating data: " << endl;
    for(int i=0; i<N; i++) {
        double x = i/100.0;
        x_data.push_back(x);
        y_data.push_back(
                exp(a*x*x + b*x + c) + rng.gaussian( w_sigma )
                );
        cout << x_data[i] << " " << y_data[i] <<endl;
    }

    // 构建最小二乘问题
    ceres::Problem problem;
    for(int i=0; i<N; i++) {
        problem.AddResidualBlock(   // 向问题中添加误差项
        // 使用自动求导，模板参数：误差类型，输出维度，输入维度，维数要与前面struct中一致
        new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3> (
                new CURVE_FITTING_COST(x_data[i], y_data[i])
                ),
        nullptr,                 // 核函数，这里不使用，为空
        abc                                 // 待估计参数
        );
    }

    // 配置求解器
    ceres::Solver::Options options;     // 这里有很多配置项可以填
    options.linear_solver_type = ceres::DENSE_QR;   // 增量方程如何求解
    options.minimizer_progress_to_stdout = true;    // 输出到out

    ceres::Solver::Summary summary;         // 优化信息
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    ceres::Solve (options, &problem, &summary);     // 开始优化
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2-t1);
    cout << "solve time cost = " << time_used.count() << "seconds. " << endl;

    // 输出结果
    cout << summary.BriefReport() << endl;
    cout << "estimated a, b, c = " ;
    for (auto a:abc) cout << a << " ";
    cout << endl;

    return 0;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77

CMakeLists.txt

cmake_minimum_required(VERSION 3.17)
project(ceres_curve_fitting)

set( CMAKE_BUILD_TYPE "Release" )
set(CMAKE_CXX_STANDARD 14)
#set( CMAKE_CXX_FLAGS "-std=c++14 -O3" )

# 添加cmake模块以使用ceres库
list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )

# 寻找Ceres库并添加它的头文件
find_package( Ceres REQUIRED )
include_directories( ${CERES_INCLUDE_DIRS} )

# OpenCV
find_package( OpenCV REQUIRED )
include_directories( ${OpenCV_DIRS} )

add_executable( ceres_curve_fitting main.cpp )
# 与Ceres和OpenCV链接
target_link_libraries( ceres_curve_fitting ${CERES_LIBRARIES} ${OpenCV_LIBS} )
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

6.4 实践：g2o

本章的第二个实践部分将介绍另一个（主要在 SLAM 领域）广为使用的优化库：g2o（General Graphic Optimization，G2O）。它是一个基于图优化的库。图优化是一种将非线性优化与图论结合起来的理论，因此在使用它之前，我们花一点篇幅介绍一个图优化理论。

6.4.1 图优化理论简介

我们已经介绍了非线性最小二乘的求解方式。它们是由很多个误差项之和组成的。然 而，仅有一组优化变量和许多个误差项，我们并不清楚它们之间的关联。比方说，某一个优化变量 $x_j$ 存在于多少个误差项里呢？我们能保证对它的优化是有意义的吗？进一步，我 们希望能够直观地看到该优化问题长什么样。于是，就说到了图优化。 图优化，是把优化问题表现成图（Graph）的一种方式。这里的图是图论意义上的图。 一个图由若干个顶点（Vertex），以及连接着这些节点的边（Edge）组成。进而，用顶点 表示优化变量，用边表示误差项。于是，对任意一个上述形式的非线性最小二乘问题，我们可以构建与之对应的一个图。

图 6-2 是一个简单的图优化例子。我们用三角形表示相机位姿节点，用圆形表示路标 点，它们构成了图优化的顶点；同时，蓝色线表示相机的运动模型，红色虚线表示观测模 型，它们构成了图优化的边。此时，虽然整个问题的数学形式仍是式（6.12）那样，但现在 我们可以直观地看到问题的结构了。如果我们希望，也可以做去掉孤立顶点或优先优化边 数较多（或按图论的术语，度数较大）的顶点这样的改进。但是最基本的图优化，是用图 模型来表达一个非线性最小二乘的优化问题。而我们可以利用图模型的某些性质，做更好 的优化。

g2o 为 SLAM 提供了图优化所需的内容。下面我们来演示一下 g2o 的使用方法。

6.4.2 g2o的编译与安装

在使用一个库之前，我们需要对它进行编译和安装。关于 g2o，读者可以从 github 下载它：https://github. com/RainerKuemmerle/g2o，或从本书提供的第三方代码库中获得。 解压代码包后，你会看到 g2o 库的所有源码，它也是一个 CMake 工程。我们先来安 装它的依赖项（部分依赖项与 Ceres 有重合）：

sudo apt-get install libqt4-dev qt4-qmake libqglviewer-dev libsuitesparse-dev libcxsparse3.1.2 libcholmod-dev
1

然后，按照 cmake 的方式对 g2o 进行编译安装即可，我们略去该过程的说明。安装完成后，g2o 的头文件将在/usr/local/g2o 下，库文件在/usr/local/lib/下。现在，我们重新 考虑 Ceres 例程中的曲线拟合实验，在 g2o 中实验一遍。

6.4.3 使用g2o拟合曲线

为了使用 g2o，首先要做的是将曲线拟合问题抽象成图优化。这个过程中，只要记住节点为优化变量，边为误差项即可。因此，曲线拟合的图优化问题可以画成图 6-3 的形式。

在曲线拟合问题中，整个问题只有一个顶点：曲线模型的参数 $a,b,c$；而每个带噪声的 数据点，构成了一个个误差项，也就是图优化的边。但这里的边与我们平时想的边不太一 样，它们是一元边（Unary Edge），即只连接一个顶点——因为我们整个图只有一个顶点。 所以在图 6-3 中，我们就只能把它画成自己连到自己的样子了。事实上，图优化中一条边 可以连接一个、两个或多个顶点，这主要反映在每个误差与多少个优化变量有关。在稍微有些玄妙的说法中，我们把它叫做超边（Hyper Edge），整个图叫做超图（Hyper Graph） x。

弄清了这个图模型之后，接下来就是在 g2o 中建立该模型，进行优化了。作为 g2o 的 用户，我们要做的事主要有以下几个步骤：

1. 定义顶点和边的类型；
2. 构件图；
3. 选择优化算法；
4. 调用g2o进行优化，返回结果。

下面来演示一下程序。

#include <iostream>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>
using namespace std; 

// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    virtual void setToOriginImpl() // 重置
    {
        _estimate << 0,0,0;
    }
    
    virtual void oplusImpl( const double* update ) // 更新
    {
        _estimate += Eigen::Vector3d(update);
    }
    // 存盘和读盘：留空
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
};

// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
    // 计算曲线模型误差
    void computeError()
    {
        const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
        const Eigen::Vector3d abc = v->estimate();
        _error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ;
    }
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
public:
    double _x;  // x 值， y 值为 _measurement
};

int main( int argc, char** argv )
{
    double a=1.0, b=2.0, c=1.0;         // 真实参数值
    int N=100;                          // 数据点
    double w_sigma=1.0;                 // 噪声Sigma值
    cv::RNG rng;                        // OpenCV随机数产生器
    double abc[3] = {0,0,0};            // abc参数的估计值

    vector<double> x_data, y_data;      // 数据
    
    cout<<"generating data: "<<endl;
    for ( int i=0; i<N; i++ )
    {
        double x = i/100.0;
        x_data.push_back ( x );
        y_data.push_back (
            exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma )
        );
        cout<<x_data[i]<<" "<<y_data[i]<<endl;
    }
    
    // 构建图优化，先设定g2o
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;  // 每个误差项优化变量维度为3，误差值维度为1
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 线性方程求解器
    Block* solver_ptr = new Block( linearSolver );      // 矩阵块求解器
    // 梯度下降方法，从GN, LM, DogLeg 中选
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
    // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
    // g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );
    g2o::SparseOptimizer optimizer;     // 图模型
    optimizer.setAlgorithm( solver );   // 设置求解器
    optimizer.setVerbose( true );       // 打开调试输出
    
    // 往图中增加顶点
    CurveFittingVertex* v = new CurveFittingVertex();
    v->setEstimate( Eigen::Vector3d(0,0,0) );
    v->setId(0);
    optimizer.addVertex( v );
    
    // 往图中增加边
    for ( int i=0; i<N; i++ )
    {
        CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
        edge->setId(i);
        edge->setVertex( 0, v );                // 设置连接的顶点
        edge->setMeasurement( y_data[i] );      // 观测数值
        edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 信息矩阵：协方差矩阵之逆
        optimizer.addEdge( edge );
    }
    
    // 执行优化
    cout<<"start optimization"<<endl;
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.initializeOptimization();
    optimizer.optimize(100);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
    cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;
    
    // 输出优化值
    Eigen::Vector3d abc_estimate = v->estimate();
    cout<<"estimated model: "<<abc_estimate.transpose()<<endl;
    
    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117

CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )
project( g2o_curve_fitting )

set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )

# 添加cmake模块以使用ceres库
list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )

# 寻找G2O
find_package( G2O REQUIRED )
include_directories( 
    ${G2O_INCLUDE_DIRS}
    "/usr/include/eigen3"
)

# OpenCV
find_package( OpenCV REQUIRED )
include_directories( ${OpenCV_DIRS} )

add_executable( curve_fitting main.cpp )
# 与G2O和OpenCV链接
target_link_libraries( curve_fitting 
    ${OpenCV_LIBS}
    g2o_core g2o_stuff
)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27

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

1. 顶点的更新函数：oplusImpl。我们知道优化过程最重要的是增量 $∆x$ 的计算，而该 函数处理的是 $x_k+1=x_k+∆x$ 的过程。

读者会觉得这并不是什么值得一提的事情，因为仅仅是个简单的加法而已，为什么 g2o 不帮我们完成呢？在曲线拟合过程中，由于优化变量（曲线参数）本身位于向量空间中，这个更新计算确实就是简单的加法。但是，当优化变量不处于向量空间中时，比方说 $x$ 是相机位姿，它本身不一定有加法运算。这时，就需要重新定义增量如何加到现有的估计上的行为了。按照第四讲的解释，我们可能使用左乘更新或右乘更新， 而不是直接的加法。
2. 顶点的重置函数：setToOriginImpl。这是平凡的，我们把估计值置零即可。
3. 边的误差计算函数：computeError。该函数需要取出边所连接的顶点的当前估计值， 根据曲线模型，与它的观测值进行比较。这和最小二乘问题中的误差模型是一致的。
4. 存盘和读盘函数：read, write。由于我们并不想进行读写操作，就留空了。

定义了顶点和边之后，我们在 main 函数里声明了一个图模型，然后按照生成的噪声 数据，往图模型中添加顶点和边，最后调用优化函数进行优化。g2o 会给出优化的结果：
我们使用 L-M 方法进行梯度下降，在迭代了 16 次后，最后优化结果与 Ceres 实验中相差无几。我们亦在程序中提供了使用 G-N 和 DogLeg 下降方式，请读者去掉它们前面的注释符号，自行对比一下各种梯度下降方法的差异。

6.5 小结

本节介绍了 SLAM 中经常碰到的一种非线性优化问题：由许多个误差项平方和组成的 最小二乘问题。我们介绍了它的定义和求解，并且讨论了两种主要的梯度下降方式：GaussNewton 和 Levenberg-Marquardt。在实践部分中，我们分别使用了 Ceres 和 g2o 两种优 化库求解同一个曲线拟合问题，发现它们给出了相似的结果。

由于我们还没有详细谈 Bundle Adjustment，所以实践部分选择了曲线拟合这样一个 简单但有代表性的例子，以演示一般的非线性最小二乘求解方式。特别地，如果用 g2o 来 拟合曲线，我们必须先把问题转换为图优化，定义新的顶点和边，这种做法是有一些迂回的——g2o 的主要目的并不在此。相比之下，Ceres 定义误差项，求曲线拟合问题则自然了 很多，因为它本身即是一个优化库。然而，在 SLAM 中，更多的问题是，一个带有许多个 相机位姿和许多个空间点的优化问题如何求解。特别地，当相机位姿以李代数表示时，误 差项关于相机位姿的导数如何计算，将是一件值得详细讨论的事。我们将在后续的章节中 发现，g2o 提供了大量的顶点和边的类型，使得它在相机位姿估计问题中非常方便。而在 Ceres 中，我们不得不自己实现每一个 Cost Function，带来了一些不便。

在实践部分的两个程序中，我们没有去计算曲线模型关于三个参数的导数，而是利用了优化库的数值求导，这使得理论和代码都会简洁一些。Ceres 库提供了基于模板元的自动求导和运行时的数值求导，而 g2o 只提供了运行时数值求导这一种方式。但是，对于大多数问题，如果我们能够推导出雅可比矩阵的解析形式并告诉优化库，就可以避免数值求导中的诸多问题。

最后，希望读者能够适应 Ceres 和 g2o 这些大量使用模板编程的方式。也许一开始会看上去比较吓人（特别是 Ceres 设置 Problem 和 g2o 初始化部分的代码），但是一旦熟悉之后，就会觉得这样的方式是自然的，而且容易扩展。我们将在 SLAM 后端章节中，继续讨论稀疏性、核函数、位姿图（Pose Graph）等问题。