视觉SLAM十四讲学习记录第六讲

第六讲非线性优化

在前面几讲，我们介绍了经典SLAM模型的运动方程和观测方程。现在我们已经知道，方程中的位姿可以由变换矩阵来描述，然后用李代数进行优化。观测方程由相机成像模型给出，其中内参是随相机固定的，而外参则是相机的位姿。于是，我们已经弄清了经典SLAM模型在视觉情况下的具体表达。
然而，由于噪声的存在，运动方程和观测方程的等式必定不是精确成立的。尽管相机可以非常好地符合针孔模型，但遗憾的是，我们得到的数据通常是受各种未知噪声影响的。即使我们有高精度的相机，运动方程和观测方程也只能近似成立。所以，与其假设数据必须符合方程，不如讨论如何在有噪声的数据中进行准确的状态估计。
解决状态估计问题需要一定程度的最优化背景知识。本节将介绍基本的无约束非线性优化方法，同时介绍优化库g2o和Ceres的使用方式。

6.1 状态估计问题

6.1.1 批量状态估计与最大后验估计

接着前面几讲的内容，我们回顾第二讲讨论的经典SLAM模型。它由一个运动方程和一个观测方程构成，如下式所示：

$\begin{cases}x_k = f(x_{k-1},u_k) + w_k\\z_{k,j} = h(y_j,x_k) + v_{k,j}\end{cases}$

通过第4讲的知识，我们了解到这里的 $x_k$ 乃是相机的位姿，可以用 $SE (3)$ 来描述。至于观测方程，第5讲已经说明，即针孔相机模型。为了让读者对它们有更深的印象，我们不妨讨论其具体参数化形式。首先，位姿变量 $x_k$ 可以由 $T_k\in SE(3)$ 表达。其次，运动方程与输入的具体形式有关，但在视觉SLAM中没有特殊性（和普通的机器人、车辆的情况一样），我们暂且不谈。观测方程则由针孔模型给定。假设在 $x_k$ 处对路标 $y_j$ 进行了一次观测，对应到图像上的像素位置 $z_{k,j}$ ，那么，观测方程可以表示成

$sz_{k,j} = K(R_ky_j+t_k)$

其中 $K$ 为相机内参， $s$ 为像素点的距离，也是 $R_ky_j+t_k)$ 的第三个分量。如果使用变换矩阵 $T_k$ 描述位姿，那么路标点 $y_j$ 必须以齐次坐标来描述，计算完成后要转换为非齐次坐标。如果你还不熟悉这个过程，请回顾第5讲。
现在，考虑数据受噪声影响后会发生什么改变。在运动和观测方程中，我们通常假设两个噪声项 $w_k,v_{k,j}$ 满足零均值的高斯分布，像这样：

$w_k\sim \mathcal{N}(0,R_k),v_k\sim \mathcal{N}(0,Q_{k,j}).$

其中 $\mathcal{N}$ 表示高斯分布，0表示零均值， $R_k,Q_{k,j}$ 为协方差矩阵。在这些噪声的影响下，我们希望通过带噪声的数据 $z$ 和 $u$ 推断位姿 $x$ 和地图 $y$ （以及它们的概率分布），这构成了一个状态估计问题。
处理这个状态估计问题的方法大致分为两种。由于在SLAM过程中，这些数据是随时间逐渐到来的，所以，我们应该持有一个当前时刻的估计状态，然后用新的数据来更新它。这种方式称为增量/渐进的方法，或者叫滤波器。在历史上很长一段时间内，研究者们使用滤波器，尤其是扩展卡尔曼滤波器及其衍生方法求解它。另一种方式，则是把数据“攒”起来一并处理，这种方式称为批量（batch）的方法。例如，我们可以把0到 $k$ 时刻所有的输入和观测数据都放在一起，问，在这样的输入和观测下，如何估计整个0到 $k$ 时刻的轨迹与地图呢？
这两种不同的处理方式引出了很多不同的估计手段。大体来说，增量方法仅关心当前时刻的状态估计 $x_k$ ，而对之前的状态则不多考虑；相对地，批量方法可以在更大的范围达到最优化，被认为优于传统的滤波器，，而成为当前视觉SLAM的主流方法。极端情况下，我们可以让机器人或无人机收集所有时刻的数据，再带回计算中心统一处理，这正是SfM（Structure from motion）的主流做法。当然，这种极端情况显然是不实时的，不符合SLAM的运用场景。所以在SLAM中，实用的方法通常是一些折衷的手段。例如，我们固定一些历史轨迹，仅对当前时刻附近的一些轨迹进行优化，这是后面要讲到的滑动窗口估计法。
理论上，批量方法更容易介绍。同时，理解了批量方法也更容易理解增量的方法。所以，本节我们重点介绍以非线性优化为主的批量优化方法，将卡尔曼滤波器及更深入的知识留到介绍后端的章节再进行讨论。由于讨论的是批量方法，考虑从1到 $N$ 的所有时刻，并假设有 $M$ 个路标点。定义所有时刻的机器人位姿和路标点坐标为

$x = \{x_1,...,x_N\}, y = \{y_1,...,y_M\}.$

同样地，用不带下标的 $u$ 表示所有时刻的输入， $z$ 表示所有时刻的观测数据。那么我们说，对机器人状态的估计，从概率学的观点来看，就是已知输入数据 $u$ 和观测数据 $z$ 的条件下，求状态 $x, y$ 的条件概率分布：

扫描二维码关注公众号，回复： 15202763 查看本文章

$P (x, y ∣ z, u) .$

特别地，当我们不知道控制输入，只有一张张的图像时，即只考虑观测方程带来的数据时，相当于估计 $P (x, y ∣ z)$ 的条件概率分布，此问题也称为SfM，即如何从许多图像中重建三维空间结构。
为了估计状态变量的条件分布，利用贝叶斯法则，有

$\frac{P(z,u|x,y) P(x,y)}{P(z,u)}\propto \underbrace{P(z,u|x,y)}_{似然}\underbrace{P(x,y)}_{先验}$

贝叶斯法则左侧称为后验概率，右侧的 $P (z ∣ x)$ 称为似然（Likehood），另一部分 $P (x)$ 称为先验（Prior）。**直接求后验分布是困难的，但是求一个状态最优估计，使得在该状态下后验概率最大化，**则是可行的：

$x,y)^{*}_{MAP} = \arg{\max{P(x,y|z,u)}} = \arg{\max{P(z,u|x,y)}}P(x,y)$

请注意贝叶斯法则的分母部分与待估计的状态 $x, y$ 无关，因而可以忽略。贝叶斯法则告诉我们，求解最大后验概率等价于最大化似然和先验的乘机。当然，我们也可以说，对不起，我不知道机器人位姿或路标大概在什么地方，此时就没有了先验。那么，可以求解最大似然估计（Maximize Likelihood Estimation，MLE）:

$x,y)^{*}_{MLE} = \arg{\max{P(z,u|x,y)}}$

直观地讲，似然是指“在现在的位姿下，可能产生怎样的观测数据”。由于我们知道观测数据，所以最大似然估计可以理解成：“在什么样的状态下，最可能产生现在观测到的数据”。这就是最大似然估计的直观意义。

6.1.2 最小二乘的引出

那么，如何求最大似然估计呢？我们说，在高斯分布的假设下，最大似然能够有效简单的形式。回顾观测模型，对于某一次观测：

$z_{k,j} = h(y_j,x_k) + v_{k,j}$

由于我们假设了噪声项 $v_k\sim\mathcal{N}(0,Q_{k,j})$ ，所以观测数据的条件概率为

$P(z_{j,k|x_k,y_j}) = N(h(y_j,x_k),Q_{k,j}).$

它依然是一个高斯分布。考虑单次观测的最大似然估计，可以使用最小化负对数来求一个高斯分布的最大似然。
我们知道高斯分布在负对数下有较好的数学形式。考虑任意高维高斯分布 $x\sim\mathcal{N}(\mu,\Sigma)$ ，它的概率密度函数展开形式为

$\frac{1}{\sqrt{(2\pi)^Ndet(\Sigma)}}\exp(-\frac{1}{2}(x - \mu)^T\Sigma^{-1}(x - \mu)).$

对其取负对数，则变为

$-\ln(P(x)) = \frac{1}{2}\ln{((2\pi)^N\det{(\Sigma)})} + \frac{1}{2}(x - \mu)^T\Sigma^{-1}(x - \mu).$

因为对数函数是单调递增的，所以对原函数求最大化相当于对负对数求最小化。在最小化上式的 $x$ 时，第一项与 $x$ 无关，可以略去。于是，只要最小化右侧的二次型项，就得到了对状态的最大似然估计。代入SLAM的观测模型，相当于在求：

$\begin{aligned}(x_k,y_j)^{*} &= \arg{\max{\mathcal{N}(h(y_j,x_k),Q_{k,j})}}\\&=\arg{\min((z_{k,j} - h(x_k,y_j))^TQ_{k,j}^{-1}(z_{k,j} - h(x_k,y_j))).}\end{aligned}$

我们发现，该式等价于最小化噪声项（即误差）的一个二次型。这个二次型称为马哈拉诺比斯距离（Mahalanobis distance），又叫马氏距离。它也可以看成由 $Q_{k,j}^{-1}$ 加权之后的欧氏距离（二范数），这里 $Q_{k,j}^{-1}$ 也叫作信息矩阵，即高斯分布协方差矩阵之逆。
现在我们考虑批量时刻的数据。通常假设各个时刻的输入和观测是相互独立的，这意味着各个输入之间是独立的，各个观测之间是独立的，并且输入和观测也是独立的。于是我们可以对联合分布进行因式分解：

$\prod\limits_kP(u_k|x_{k-1},x_k)\prod\limits_{k,j}P(z_{k,j}|x_k,y_j),$

这说明我们可以独立地处理各时刻的运动和观测。定义各次输入和观测数据与模型之间的误差：

$e_{u,k} = x_k - f(x_{k-1},u_k)$
$e_{z,j,k} = z_{k,j} - h(x_k,y_j),$

那么，最小化所有时刻估计值与真实读数之间的马氏距离，等价于求最大似然估计。负对数允许我们把乘积变成求和：

$\min{J(x,y) = \sum\limits_k}{e^T_{u,k}R_k^{-1}e_{u,k} + \sum\limits_k\sum\limits_j{e^T_{z,k,j}Q^{-1}_{k,j}e_{z,k,j}}}$

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

首先，整个问题的目标函数由许多个误差的（加权的）二次型组成。虽然总体的状态变量维数很高，但每个误差项都是简单的，仅与一两个状态变量有关。例如，运动误差只与 $x_{k-1},x_k$
有关，观测误差只与 $x_k,y_k$ 有关。这种关系会让整个问题有一种稀疏的形式，我们将在介绍后端的章节中看到。
其次，如果使用李代数表示增量，则该问题是无约束的最小二乘问题。但如果用旋转矩阵/变换矩阵描述位姿，则会引入旋转矩阵自身的约束，即需在问题中加入 $s.t.R^TR = I$ 且 $det{(R)} = 1$ 这样令人头大的条件。额外的约束会使优化变得更困难。这体现了李代数的优势。
最后，我们使用了二次型度量误差。误差的分布将影响此项在整个问题中的权重。例如，某次的观测非常准确，那么协方差矩阵就会“小”，而信息矩阵就会“大”，所以这个误差项会在整个问题中占有较高的权重。我们之后也会看到它存在一些问题，但是目前先不讨论。

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

6.1.3 例子：批量状态估计

笔者发现在这举一个简单的例子会更好一些。考虑一个非常简单的离散时间系统：

$x_k = x_{k-1} + u_k + w_k,\;\;\;\;w_k\sim\mathcal{N}(0,Q_k)$
$z_k = x_{k} + n_k,\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;n_k\sim\mathcal{N}(0,R_k)$
这可以表达一辆沿 $x$ 轴前进或后退的汽车。第一个公式为运动方程， $u_k$ 为输入， $w_k$ 为噪声；第二个公式为观测方程， $z_k$ 为汽车位置的测量。取时间 $k = 1, .., 3$ ，现希望根据已有的 $v, y$ 进行状态估计。设初始状态 $x_0$ 为已知。下面来推导批量状态的最大似然估计。
首先，令批量状态变量为 $x = [x_0,x_1,x_2,x_3]^T$ ，；令批量观测为 $z = [z_1,z_2,z_3]^T$ ,按同样方式定义 $u = [u_1,u_2,u_3]^T$ 。按照先前的推导，我们知道最大似然估计为

$\begin{aligned}x_{map}^{*} &= \arg{\max{P(x|u,z)}} = \arg\max P(u,z|x)\\&=\prod\limits^3_{k = 1}P(u_k|x_{k-1},x_k)\prod\limits^3_{k = 1}P(z_k|x_k)\end{aligned}$

对于具体的每一项，比如运动方程，我们知道：

$P(u_k|x_{k-1},x_k) = \mathcal{N}(x_k - x_{k-1},Q_k),$

观测方程也是类似的：

$P(z_k|x_k) = \mathcal{N}(x_k,R_k).$

根据这些方法，我们就能实际地解决上面地批量状态估计问题。根据之前地叙述，可以构建误差变量：

$e_{u,k} = x_k - x_{k-1} - u_k,\;\;\;e_{z,k} = z_k - x_k,$

于是最小二乘的目标函数为：

$\min\sum\limits_{k=1}^3e_{u,k}^TQ_k^{-1}e_{u,k} + \sum\limits_{k=1}^3e_{z,k}^TR_k^{-1}e_{z,k}.$

此外，这个系统是线性系统，我们可以很容易地将它写成向量形式。定义向量 $y = [u,z]^T$ ，那么可以写出矩阵 $H$ ,使得

$e\sim\mathcal{N}(0,\Sigma).$

那么：

$\begin{bmatrix}1&-1&0&0\\\\0&1&-1&0\\\\0&0&1&-1\\\\0&1&0&0\\\\0&0&1&0\\\\0&0&0&1\end{bmatrix}$

且 $\Sigma = diag(Q_1,Q_2,Q_3,R_1,R_2,R_3)$ 。整个问题可以写成

$x_{map}^{*} = \arg\min e^T\Sigma^{-1}e,$

之后我们将看到，这个问题有唯一的解：

$x_{map}^{*} = (H^T\Sigma^{-1}H)^{-1}H^T\Sigma^{-1}y.$

6.2 非线性最小二乘

先来考虑一个简单的最小二乘问题：

$\min\limits_xF(x) = \frac{1}{2}||f(x)||_2^2.$

其中，自变量 $x\in\mathbb{R}^n,$ $f$ 是任意标量非线性函数 $f(x):\mathbb{R}^n\rightarrow\mathbb{R}。$ 注意这里的系数 $\frac{1}{2}$ 是无关紧要的，有些文献上带有这个系数，有些文献则不带，它不会影响之后的结论。下面讨论如何求解这样一个优化问题。显然，如果 $f$ 是个数学形式上很简单的函数，那么该问题可以用解析形式来求。令目标函数的导数为零，然后求解 $x$ 的最优值，就和求二元函数的极值一样：

$\frac{dF}{dx} = 0.$

解此方程，就得到了导数为零处的极值。它们可能是极大、极小或鞍点处的值，只要逐个比较它们的函数值大小即可。但是，这个方程是否容易求解呢？这取决于 $f$ 导函数的形式。如果 $f$ 为简单的线性函数，这么这个问题就是简单的线性最小二乘问题，但是有些导函数可能形式复杂，使得该方程可能不容易求解。求解这个方程需要我们知道关于目标函数的全局性质，而通常这是不大可能的。对于不方便直接求解的最小二乘问题，我们可以用迭代的方式，从一个初始值出发，不断地更新当前地优化变量，使目标函数下降。具体步骤可列写如下：

给定某个初始值 $x_0$
对于第 $k$ 次迭代，寻找一个增量 $\Delta x_k，$ 使得 $||f(x_k + \Delta x_k)^2_2||$ 达到极小值。
若 $\Delta x_k$ 足够小，则停止。
否则，令 $x_{k+1} = x_k + \Delta x_k$ ，返回第2步。

这让求解导函数为零的问题变成了一个不断寻找下降增量 $\Delta x_k$ 的问题，我们将看到，由于可以对 $f$ 进行线性化，增量的计算将简单很多。当函数下降直到增量非常小的时候就认为算法收敛，目标函数达到了一个极小值。在这个过程中，问题在于如何找到每次迭代点的增量，而这是一个局部的问题，我们只需要关心 $f$ 在迭代值处的局部性质而非全局性质。这类方法在最优化、机器学习等领域应用非常广泛。
接下来，我们考察如何寻找这个增量 $\Delta x_k。$ 这部分知识实际属于数值优化的领域，我们来看一些广泛使用的结果。

6.2.1 一阶和二阶梯度法

现在考虑第 $k$ 次迭代，假设我们在 $x_k$ 处，想要寻到增量 $\Delta x_k$ ，那么最直观的方式是将目标函数在 $x_k$ 附近进行泰勒展开：

$F(x_k + \Delta x_k) ≈ F(x_k) + J(x_k)^T\Delta x_k + \frac{1}{2}\Delta x^T_kH(x_k)\Delta x_k.$

其中 $J(x_k)$ 是 $F (x)$ 关于 $x$ 的一阶导数[也叫梯度、雅可比(jacobian) 矩阵]， $H$ 则是二阶导数[*海塞(Hessian)**矩阵]，它们都在 $x_k$ 处取值，读者应该在大学本科多元微积分课程中学习过。我们可以选择保留泰勒展开的一阶或二阶项，那么对应的求解方法则称为一阶梯度或二阶梯度法。如果保留一阶梯度，那么取增量为反向的梯度，则可保证函数下降：

$\Delta x^{*} = -J(x_k).$

当然这只是个方向，通常我们还要再指定一个步长 $\lambda$ 。步长可以根据一定的条件来计算，在机器学习中也有一些经验性质的方法，但我们不展开谈。这种方法被称为最速下降法。它的直观意义非常简单，只要我们沿着反向梯度方向前进，在一阶(线性)的近似下，目标函数必定会下降。
注意，以上讨论都是在第 $k$ 次迭代时进行的，并不涉及其他的迭代信息。所以为了简化符号，后面我们省略下标 $k$ ，并认为这些讨论对任意一次迭代都成立。
另外，我们可选择保留二阶梯度信息，此时增量方程为

$\Delta x^{*} = \arg\min(F(x) + J(x)^T\Delta x + \frac{1}{2}\Delta x^T H\Delta x).$

右侧只含 $\Delta x$ 的零次、一次和二次项。求右侧等式关于 $\Delta x$ 的导数并令它为零，得到

$H\Delta x = 0 \Rightarrow H\Delta x = -J.$

求解这个线性方程，就得到了增量。该方法又称为牛顿法。
我们看到，一阶和二阶梯度法都十分直观，只要把函数在迭代点附近进行泰勒展开，并针对更新量做最小化即可。事实上，我们用一次或二次的函数近似了原函数，然后用近似函数的最小值猜测原函数的极小值。只要原目标函数局部看起来像一次或二次函数，这类算法就是成立的（这也是现实中的情形）。不过，这两种方法也存在它们自身的问题。最速下降法过于贪心，容易走出锯齿路线，反而增加了迭代次数。而牛顿法则需要计算目标函数的 $H$ 矩阵，这在问题规模较大时非常困难，我们通常倾向于避免 $H$ 的计算。对于一般的问题，一些拟牛顿法可以得到较好的结果，而对于最小二乘问题，还有几类更实用的方法：高斯牛顿法和列文伯格——马夸尔特方法。

6.2.2 高斯牛顿法

高斯牛顿法是最优化算法中最简单的方法之一。它的思想是将 $f (x)$ 进行一阶的泰勒展开。请注意这里不是目标函数 $F (x)$ 而是 $f (x)$ ，否则就变成牛顿法。

$\Delta x) ≈ f(x) + J(x)^T\Delta x.$

这里 $J(x)^T$ 为 $f (x)$ 关于 $x$ 的导数，为 $n \times 1$ 的列向量。根据前面的框架，当前的目标是寻找增量 $\Delta x$ ,使得 $\Delta x)||^2$ 达到最小。为了求 $\Delta x$ ，我们需要解一个线性的最小二乘问题：

$\Delta x^{*} = \arg\min\limits_{\Delta x}\frac{1}{2}||f(x) + J(x)^T\Delta x ||^2.$

这个方程与之前的有什么不一样呢？根据极值条件，将上述目标函数对 $\Delta x$ 求导，并令导数为零。为此，先展开目标函数的平方项：

$\begin{aligned}\frac{1}{2}||f(x)+J(x)^T\Delta x||^2&=\frac{1}{2}(f(x)+J(x)^T\Delta x)^T(f(x)+J(x)^T\Delta x)\\&=\frac{1}{2}(||f(x)||^2_2 + 2f(x)J(x)^T\Delta x + \Delta x^TJ(x)J(x)^T\Delta x).\end{aligned}$

求上式关于 $\Delta x$ 的导数，并令其为零：

$J(x)J^T(x)\Delta x = 0$

可以得到如下方程组：

$\underbrace{J(x)J^T}_{H(x)}(x)\Delta x = \underbrace{-J(x)f(x)}_{g(x)}$

这个方程是关于变量 $\Delta x$ 的线性方程组，我们称它为增量方程，也可以称为高斯牛顿方程（Gauss-Newton equation）或者正规方程（Normal equation）。我们把左边的系数定义为 $H$ ,右边定义为 $g$ ，那么上式变为

$H\Delta x = g$

这里把左侧记作 $H$ 是有意义的。对比牛顿法可见，高斯牛顿法用 $JJ^T$ 作为牛顿法中二阶Hessian矩阵的近似，从而省略了计算 $H$ 的过程。求解增量方程是整个优化问题的核心所在。如果我们能够顺利解出该方程，那么高斯牛顿法的算法步骤可以写成：

给定初始值 $x_0$ 。
对于第 $k$ 次迭代，求出当前的雅可比矩阵 $J(x_k)$ 和误差 $f(x_k)$
求解增量方程： $H\Delta x_k = g$
若 $\Delta x_k$ 足够小，则停止。否则，令 $x_{k+1} = x_k + \Delta x_k,$ 返回第2步。

从算法步骤中可以看到，增量方程的求解占据着主要地位。只要我们能够顺利解出增量，就能保证目标函数能够正确地下降。
为了求增量方程，我们需要求解 $H^{-1}，$ 这需要 $H$ 矩阵可逆，但实际数据中计算得到的 $JJ^T$ 却只有半正定性。也就是说，在使用高斯牛顿法时，可能出现 $JJ^T$ 为奇异矩阵或者病态(ill-condition)的情况，此时增量的稳定性较差，导致算法不收敛。直观地说，原函数在这个点的局部近似不像一个二次函数。更严重的是，就算我们假设 $H$ 非奇异也非病态，如果我们求出来的步长 $\Delta x$ 太大，也会导致我们采用的局部近似式不够准确，这样一来我们甚至无法保证它的迭代收敛，哪怕是让目标函数变得更大都是有可能的。
尽管高斯牛顿法有这些缺点，但它依然算是非线性优化方面一种简单有效的方法，值得我们学习。在非线性优化领域，相当多的算法都可以归结为高斯牛顿法的变种。这些算法都借助了高斯牛顿法的思想并且通过自己的改进修正其缺点。例如一些线搜索方法（Line Search Method）加入了一个步长 $\alpha$ ，在确定了 $\Delta x$ 后进一步找到 $\alpha$ 使得 $\alpha\Delta x)||^2$ 达到最小，而不是简单地令 $\alpha = 1$ 。
列文伯格——马夸尔特方法在一定程度上修正了这些问题。一般认为它比高斯牛顿法更为健壮，但它的收敛速度可能比高斯牛顿法更慢，被称为阻尼牛顿法(Damped Newton Method)。

6.2.3 列文伯格——马夸尔特方法

高斯牛顿法中采用的近似二阶泰勒展开只能在展开点附近有较好的近似效果，所以我们很自然地想到应该给 $\Delta x$ 添加一个范围，称为信赖区域(Trust Region)。这个范围定义了在什么情况下二阶近似是有效的，这类方法也称为信赖区域方法(Trust Region Method)。在信赖区域里，我们认为近似是有效的；出了这个区域，近似可能会出问题。
那么，如何确定这个信赖区域的范围呢？一个比较好的方法是根据我们的近似模型和实际函数之间的差异来确定：如果差异小，说明近似效果好，我们扩大近似的范围；反之，如果差异大，就缩小近似的范围。我们定义一个指标 $\rho$ 来刻画近似的好坏程度：

$\rho = \frac{f(x + \Delta x) - f(x)}{J(x)^T\Delta x}.$

$\rho$ 的分子是实际函数下降的值，分母是近似模型下降的值。如果 $\rho$ 接近于1，则近似是好的。如果 $\rho$ 太小，说明实际减少的值远少于近似减小的值，则认为近似比较差，需要缩小近似范围。反之，如果 $\rho$ 比较大，则说明实际下降的比预计的更大，我们可以放大近似范围。
于是，我们构建一个改良版的非线性优化框架，该框架会比高斯牛顿法有更好的效果：

给定初始值 $x_0$ ，以及初始优化半径 $\mu$ 。
对于第 $k$ 次迭代，在高斯牛顿法的基础上加上信赖区域，求解：
$\min\limits_{\Delta x_k}\frac{1}{2}||f(x_k) + J(x_k)^T\Delta x_k||^2, s.t. ||D\Delta x_k||^2 \leqslant \mu,$
其中， $\mu$ 是信赖区域的半径， $D$ 为系数矩阵，将在后文说明。
按 $\rho = \frac{f(x + \Delta x) - f(x)}{J(x)^T\Delta x}$ 计算
若 $\rho > \frac{3}{4}$ ，则设置 $\mu = 2\mu$ 。
若 $\rho < \frac{1}{4}$ ，则设置 $\mu = 0.5\mu$ 。
如果 $\rho$ 大于某阈值，则认为近似可行。令 $x_{k+1} = x_k+ \Delta x_k$ 。
判断算法是否收敛。如不收敛则返回第2步，否则结束。

这里近似范围扩大的倍数和阈值都是经验值，可以替换成别的数值。在2的式子中，我们把增量限定于一个半径为 $\mu$ 的球中，认为只有在这个球内才是有效的。带上 $D$ 之后，这个球可以看成一个椭球。在列文伯格提出的优化方法中，把 $D$ 取成单位阵 $I$ ，相当于直接把 $\Delta x_k$ 约束在一个球中。随后，马夸尔特提出将 $D$ 取成非负数对角阵——实际中通常用 $J^TJ$ 的对角元素平方根，使得在梯度小的维度上约束范围更大一些。
无论如何，在列文伯格——马夸尔特优化中，我们都需要解上式那样一个子问题来获得梯度。这个子问题是带不等式约束的优化问题，我们用拉格朗日乘子把约束项放到目标函数中，构成拉格朗日函数：

$\mathcal{L}(\Delta x_k,\lambda) =\frac{1}{2}||f(x_k) + J(x_k)^T\Delta x_k||^2 + \frac{\lambda}{2}(||D\Delta x_k||^2 - \mu).$

这里 $\lambda$ 为拉格朗日乘子。类似于高斯牛顿法中的做法，令该拉格朗日函数关于 $\Delta x$ 的导数为零，它的核心仍是计算增量的线性方程：

$\lambda D^TD)\Delta x_k = g.$

可以看到，相比于高斯牛顿法，增量方程多了一项 $\lambda D^TD$ 。如果考虑它的简化形式，即 $D = I$ ，那么相当于求解：

$\lambda I)\Delta x_k = g.$

我们看到，一方面，当参数 $\lambda$ 比较小时， $H$ 占主要地位，这说明二次近似模型在该范围内是比较好的，列文伯格——马夸尔特方法更接近于高斯牛顿法。另一方面，当 $\lambda$ 比较大时， $\lambda I$ 占据主要地位，列文伯格——马夸尔特方法更接近于一阶梯度下降法（即最速下降），这说明附近的二次近似不够好。列文伯格——马夸尔特方法的求解方式，可在一定程度上避免线性方程组的系数矩阵的非奇异和病态问题，提供更稳定、更准确的增量 $\Delta x$ 。
在实际中，还存在许多其他的方式来求解增量，例如 $Do g - L e g$ 等方法。我们在这里所介绍的，只是最常见而且最基本的方法，也是视觉SLAM中用得最多的方法。实际问题中，我们通常选择高斯牛顿法或列文伯格——马夸尔特方法中的一种作为梯度下降策略。当问题性质较好时，用高斯牛顿。如果问题接近病态，则用列文伯格——马夸尔特方法。

小结

由于不希望本书变成一本让人头疼的数学教科书。所以这里只罗列了最常见的两种非线性优化方案——高斯牛顿法和列文伯格——马夸尔特方法。我们避开了许多数学性质上的讨论。如果读者对优化感兴趣，可以进一步阅读专门介绍数值优化的书籍（显然这是一个很大的课题）。以高斯牛顿法和列文伯格——马夸尔特方法为代表的优化方法，在很多开源的优化库中都已经实现并提供给用户，我们会在下文进行实验。最优化是处理许多实际问题的基本数学工具，不光在视觉SLAM中起着核心作用，在类似于深度学习等其他领域，它也是求解问题的核心方法之一（深度学习数据量很大，以一阶方法为主）。我们希望读者能根据自身能力，去了解更多的最优化算法。
也许你发现了，无论是高斯牛顿法还是列文伯格——马夸尔特方法，在做最优化计算时，都需要提供变量的初始值。你也许会问，这个初始值能否随意设置？当然不能。实际上，非线性优化的所有迭代求解方案，都需要用户提供一个良好的初始值。由于目标函数太复杂，导致在求解空间上的变化难以预测，对问题提供不同的初始值往往会导致不同的计算结果。这种情况是非线性优化的通病：大多数算法都容易陷入局部极小值。因此，无论是哪类科学问题，我们提供初始值都应该有科学依据，例如视觉SLAM问题中，我们会用ICP、PnP之类的算法提供优化初始值。总之，一个良好的初始值对最优化问题非常重要！
也许读者还会对上面提到的最优化产生疑问：如何求解线性增量方程组呢？我们只讲到了增量方程是一个线性方程。但是直接对系数矩阵进行求逆岂不是要进行大量的计算？当然不是。在视觉SLAM算法里，经常遇到 $\Delta x$ 的维度大到好几百或者上千，如果你要做大规模的视觉三维重建，就会经常发现这个维度可以轻易达到几十万甚至更高的级别，要对那么大个矩阵进行求逆是大多数处理器无法负担的，因此存在着许多针对线性方程组的数值求解方法。在不同的领域有不同的求解方式，但几乎没有一种方式是直接求系数矩阵的逆，我们会采用矩阵分解的方法来解线性方程，例如QR、Cholesky等分解方法。这些方法通常在矩阵论等教科书中可以找到，我们不多加介绍。幸运的是，视觉SLAM里这个矩阵往往有特定的稀疏形式，这为实时求解优化问题提供了可能性。我们将在第9讲中详细介绍它的原理。利用稀疏形式的消元、分解，最后进行求解增量，会让求解的效率大大提高。在很多开源的优化库上，维度为一万多的变量在一般的计算机上就可以在几秒甚至更短的时间内被求解出来，其原因也是用了更加高级的数学工具。视觉SLAM算法现在能够实时地实现，也多亏了系数矩阵是稀疏的，如果矩阵是稠密的，恐怕优化这类视觉SLAM算法就不会被学界广泛采纳了。

6.3 实践：曲线拟合问题

6.3.1 手写高斯牛顿法

接下来，我们用一个简单的例子来说明如何求解最小二乘问题。我们将演示如何手写高斯牛顿法，然后介绍如何使用优化库求解此问题。对于同一个问题，这些实现方式会得到同样的结果，因为它们的核心算法是一样的。
考虑一条满足以下方程的曲线：

$y = \exp{(ax^2 + bx + c)} + w,$

其中， $a, b, c$ 为曲线的参数， $w$ 为高斯噪声，满足 $w\sim(0,\sigma^2)。$ 我们故意选择了一个非线性模型，使问题不至于太简单。现在，假设我们有 $N$ 个关于 $x, y$ 的观测数据点，想根据这些数据点求出曲线的参数。那么，可以求解下面的最小二乘问题以估计曲线参数：

$\min\limits_{a,b,c}\frac{1}{2}\sum\limits_{i = 1}^N||y_i - \exp(ax_i^2 + bx_i + c)||^2.$

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

$e_i = y_i - \exp{(ax_i^2 + bx_i + c)} ,$

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

$\frac{\partial e_i} {\partial a} = -x_i^2\exp(ax_i^2 + bx_i + c)^2$

$\frac{\partial e_i} {\partial b} = -x_i\exp(ax_i^2 + bx_i + c)^2$

$\frac{\partial e_i} {\partial c} = -\exp(ax_i^2 + bx_i + c)^2$

于是 $J_i = [\frac{\partial e_i} {\partial a},\frac{\partial e_i} {\partial b},\frac{\partial e_i} {\partial c}]^T$ ，高斯牛顿法的增量方程为

$\left(\sum\limits_{i = 1}^{100}J_i(\sigma^2)^{-1}J_i^T\right)\Delta x_k = \sum\limits_{i = 1}^{100}-J_i(\sigma^2)^{-1}e_i,$

当然，我们也可以选择把所有的 $J_i$ 排成一列，将这个方程写成矩阵形式，不过它的含义与求和形式是一致的。下面的代码演示了这个过程是如何进行的。

#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

int main(int argc, char **argv) {
    
    
	double ar = 1.0, br = 2.0, cr = 1.0; // 真实参数值
	double ae = 2.0, be = -1.0, ce = 5.0;// 估计参数值
	int N = 100;						 // 数据点
	double w_sigma = 1.0;				 // 噪声Sigma值
	double inv_sigma = 1.0 / w_sigma;
	cv::RNG rng;						 // OpenCV随机数产生器

	Vector<double> x_data, y_data;		 // 数据
	for (int i = 0; i < N;i++) {
    
    
		double x = i / 100.0;
		x_data.push_back(x);
		y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
	}

	// 开始Gauss-Newton迭代
	int iterations = 100;				 // 迭代次数
	double cost = 0, lastCost = 0;		 // 本次迭代的cost和上一次迭代的cost

	chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
	for (int iter = 0; iter < iterations; iter++) {
    
    
		Matrix3d H = Matrix3d::Zero();   // Hessian = J^T W^{-1} J in Gauss-Newton
		Vector3d b = Vector3d::Zero();	 // bias
		cost = 0;

		for (int i = 0;i < N; i++) {
    
    
			double xi = x_data[i], yi = y_data[i]; // 第i个数据点
			double error = yi - exp(ae * xi * xi + be * xi + ce);
			Vector3d J;							   // 雅可比矩阵
			J[0] = - xi * xi * exp(ae * xi * xi + be * xi + ce); // de/da
			J[1] = - xi * exp(ae * xi * xi + be * xi + ce); // de/db
			J[2] = - exp(ae * xi * xi + be * xi + ce); // de/dc

			H += inv_sigma * inv_sigma * J * J.transpose();
			b += - inv_sigma * inv_sigma * error * J;
			cost += error * error;
		}
	   // 求解线性方程 Hx = b
	   Vector3d dx = H.ldlt().solve(b);
	   if (isnan(dx[0])) {
    
    
	   		cout << "result is nan!" << endl;
	   		break;
	   }

		if (iter > 0 && cost >= lastCost) {
    
    
			cout << "cost: " << cost << ">= last cost:" << lastCost << ",break." << endl;
			break;
		}
		
		ae += dx[0];
		be += dx[1];
		ce += dx[2];

		lastCost = cost;

		cout << "total cost: " << cost << ", \t\tupdate: " << dx.transpose() <<
		  "\t\testimated params: " << ae << "," << be << "," << ce << endl;
	}	

	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 << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
	return 0;
}

整个问题的目标函数在迭代9次之后趋近收敛，更新量趋近于零。最终估计的值与真值接近，函数图像如下图所示。下面我们尝试使用优化库来完成同样的任务。
在这里插入图片描述

6.3.2 使用Ceres进行曲线拟合

本节向大家介绍两个C++的优化库：来自谷歌的Ceres库及基于图优化的g2o库。因为使用g2o还需要介绍一点图优化的相关知识，，所以我们先来介绍Ceres，然后介绍一些图优化理论，最后来g2o。因为优化算法在之后的“视觉里程计”和“后端”中都会出现，所以请读者务必掌握优化算法的意义，理解程序的内容。

Ceres 简介

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

$\min\limits_x\frac{1}{2}\sum\limits_i\rho_i\left(||f_i(x_{i_1},...,x_{i_n})||^2\right)$

$\;\;l_j \leqslant x_j\leqslant u_j.$

在这个问题中， $x_1,...,x_n$ 为优化变量，又称参数块（Parameter blocks）， $f_i$ 称为代价函数（Cost function），也称为残差块（Residual blocks），在SLAM中也可理解为误差项。 $l_j$ 和 $u_j$ 为第 $j$ 个优化变量的上限和下限。在最简单的情况下，取 $l_j = -\infty,u_j=\infty$ （不限制优化变量的边界）。此时，目标函数由许多平方项经过一个核函数 $\rho(·)$ 之后求和组成。同样，可以取 $\rho$ 为恒等函数，那么目标函数即为许多项的平方和，我们就得到了无约束的最小二乘问题，和先前介绍的理论是一致的。
为了让Ceres帮我们求解这个问题，我们需要做以下几件事：

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

下面，我们来实际操作如何用Ceres求解曲线拟合问题，理解优化的过程。

安装 Ceres

为了使用Ceres，我们需要对它进行编译安装。Ceres的GitHub地址，你也可以直接使用本书代码3rdparty目录里的Ceres，这样就可以和作者完全同一个版本。
与之前碰到的库一样，Ceres是一个cmake工程。先来安装它的依赖项，在Ubuntu系统中可以用apt-get安装，主要是谷歌自己使用的一些日志和测试工具：

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求解同样的问题。

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

using namespace std;

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

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

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

int main(int argc,char **argv){
    
    
	double ar = 1.0, br = 2.0, cr = 1.0; // 真实参数值
	double ae = 2.0, be = -1.0, ce = 5.0;// 估计参数值
	int N = 100;						 // 数据点
	double w_sigma = 1.0;				 // 噪声Sigma值
	double inv_sigma = 1.0 / w_sigma;
	cv::RNG rng;						 // OpenCV随机数产生器

	Vector<double> x_data, y_data;		 // 数据
	for (int i = 0; i < N;i++) {
    
    
		double x = i / 100.0;
		x_data.push_back(x);
		y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
	}
	
	double abc[3] = {
    
    ae, be, ce};

	// 构建最小二乘问题
	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_datda[i])
			),
			nullptr,		// 核函数，这里不使用，为空
			abc				// 待估计参数
		);
	}

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

	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;
}

程序中需要说明的地方均已加注释。可以看到，我们利用OpenCV的噪声生成器生成了10个带高斯噪声的数据，随后利用Ceres进行拟合。这里演示的Ceres用法有如下几项：

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

最后，如果要查看实验结果，调用build/ceresCurveFitting查看优化结果。
最终的优化值和我们上一节的实验结果基本相同，但运行速度上Ceres要相对慢一些。在作者的计算机上Ceres约使用了1.3毫秒，这比手写高斯牛顿法慢了约六倍。
希望读者通过这个简单的例子对Ceres的使用方法有一个大致了解。它的优点是提供了全自动求导工具，使得不必去计算很麻烦的雅可比矩阵。Ceres的自动求导是通过模板元实现的，在编译时期就可以完成自动求导工作，不过仍然是数值导数。本书大部分时候仍然会介绍雅可比矩阵的计算，因为那样对理解问题更有帮助，而且在优化中更少出现问题。此外，Ceres的优化过程配置也很丰富，使其适合很广泛的最小二乘优化问题，包括SLAM之外的各种问题。

6.3.3 使用g2o进行曲线拟合

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

图优化理论简介

我们已经介绍了非线性最小二乘的求解方式。它们是由很多个误差项之和组成的。然而，目标函数仅描述了优化变量和许多个误差项，但我们尚不清楚它们之间的关联。例如，某个优化变量 $x_j$ 存在于多少个误差项中呢？我们能保证对它的优化是有意义的吗？进一步，我们希望能够直观地看到该优化问题长什么样。于是，就牵涉到了图优化。
图优化，是把优化问题表现成图的一种方式。这里的图是图论意义上的图。一个图由若干个顶点（Vertex），以及连接着这些顶点的边（Edge）组成。进而，用顶点表示优化变量，用边表示误差项。于是，对任意一个上述形式的非线性最小二乘问题，我们可以构建与之对应的一个图。我们可以简单地称它为图，也可以用概率图里的定义，称之为贝叶斯图或因子图。
下图是一个简单的图优化例子。我们用三角形表示相机位姿节点，用圆形表示路标点，它们构成了图优化的顶点；同时，实线表示相机的运动模型，虚线表示观测模型，它们构成了图优化的边。此时，虽然整个问题的数学形式仍然是

$\min{J(x,y) = \sum\limits_k}{e^T_{u,k}R_k^{-1}e_{u,k} + \sum\limits_k\sum\limits_j{e^T_{z,k,j}Q^{-1}_{k,j}e_{z,k,j}}}$

但现在我们可以直观地看到问题的结构了。如果希望，也可以做去掉孤立顶点或优先优化边数较多（或按图论的术语，度数较大）的顶点这样的改进。但是最基本的图优化是用图模型来表达一个非线性最小二乘的优化问题。而我们可以利用图模型的某些性质做更好的优化。
在这里插入图片描述
g2o是一个通用的图优化库。“通用”意味着你可以在g2o里求解任何能够表示为图优化的最小二乘问题，显然包括上面谈的曲线拟合问题。下面我们来演示这个问题。

g2o的编译与安装

在使用一个库之前，我们需要对它进行编译和安装。读者应该已经体验过很多次这种过程了，它们基本大同小异。关于g2o，读者可以从GitHub下载它：g2o，或从分享的第三方代码库中获得。由于g2o还在继续更新，所以还是建议你使用3rdparty下的g2o，以保证版本与笔者的相同。
g2o也是一个cmake工程。我们先来安装它的依赖项（部分依赖项与Ceres重合）：

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

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

使用g2o拟合曲线

为了使用g2o，首先要将曲线拟合问题抽象成图优化。这个过程中，只要记住节点为优化变量，边为误差项即可。曲线拟合对应的图优化模型可以如下图所示：

在这里插入图片描述
在曲线拟合问题中，整个问题只有一个顶点：曲线模型的参数是 $a, b, c ；$ 而各个带噪声的数据点，构成了一个个误差项，也就是图优化的边。但这里的边与我们平时想的边不太一样，它们是一元边（Unary Edge），即只连接一个顶点——因为整个图只有一个顶点。所以在上图中，我们只能把它画成自己连到自己的样子。事实上，图优化中一条边可以连接一个、两个或多个顶点，这主要反映每个误差与多少个优化变量有关。在稍有些玄妙的说法中，我们把它叫作超边（Hyper Edge），整个图叫作超图（Hyper Graph）。
弄清了这个图模型之后，接下来就是在g2o中建立该模型进行优化。作为g2o的用户，我们要做的事主要包含以下步骤：

定义顶点和边的类型。
构建图。
选择优化算法。
调用g2o进行优化，返回结果。
这部分和Ceres是非常相似的，当然程序在书写上会有一些不同。

#include <iostream>
#include <g2o/core/g2o_core_api.h>
#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() override {
    
    
		_estimate << 0,	0, 0;
	}

	// 更新
	virtual void oplusImpl(const double *update) override {
    
    
		_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) {
    
    }
	// 计算曲线模型误差
	virtual void computeError() override {
    
    
		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 void linearizeOplus() override {
    
    
		const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
		const Eigen::Vector3d abc = v->estimate();
		double y = exp(abc[0] * _x * _x + abc[1] * _x + abc[2]);
		_jacobianOplusXi[0] = -_x * _x * y;
		_jacobianOplusXi[1] = -_x * y;
		_jacobianOplusXi[2] = -y;
	}

	virtual bool read(istream &in) {
    
    }
	virtual bool write(ostream &out) const {
    
    }
public:
	double _x;	// x值， y值为_measurement
};

int main(int argc, char **argv) {
    
    
	// 省略数据生成部分代码
	// 构建图优化，先设定g2o
	typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType; // 每个误差项优化变量维度为3，误差值维度为1
	typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型

	// 梯度下降方法， 可以从GN、LM、DogLeg中选
	auto solver = new g2o::OptimizationAlgorithmGaussNewton(
		g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
	g2o::SparseOptimizer optimizer;		// 图模型
	optimizer.setAlgorithm(solver);		// 设置求解器
	optimizer.setVerbose(true);			// 打开调试输出
	
	// 往图中增加顶点
	CurveFittingVertex *v = new CurveFittingVertex();
	v->setEstimate(Eigen::Vector3d(ae, be, ce));
	v->setId(0);
	opytimizer.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(10);
	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;

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

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

定义了顶点和边之后，我们在main函数里声明了一个图模型，然后按照生成的噪声数据，往图模型中添加顶点和边，最后调用优化函数进行优化。g2o会给出优化的结果。
使用高斯——牛顿方法进行梯度下降，在迭代了9次后得到优化结果，与Ceres和手写高斯牛顿法相差无几。从运行速度来看，我们的实验结论是手写快于g2o，而g2o快于Ceres。这是一个大体符合直觉的经验，通用性和高效性往往是互相矛盾的。但是本实验中Ceres使用了自动求导，且求解器配置与高斯牛顿还不完全一致，所以看起来慢一些。

6.4 小结

本节介绍了SLAM中经常碰到的一种非线性优化问题：由许多误差项平方和组成的最小二乘问题。我们介绍了它的定义和求解，并且讨论了两种主要的梯度下降方式：高斯牛顿法和列文伯格——马夸尔特方法。在实践部分中，分别使用了手写高斯牛顿法、Ceres和g2o两种优化库求解同一个曲线拟合问题，发现它们给出了相似的结果。
由于还没有详细谈Bundle Adjustment，我们在实践部分选择了曲线拟合这样一个简单但有代表性的例子，以演示一般的非线性最小二乘求解方式。特别地，如果用g2o来拟合曲线，必须先把问题转换为图优化，定义新的顶点和边，这种做法是有一些迂回的——g2o的主要目的并不在此。相比之下，Ceres定义误差项求曲线拟合问题则自然了很多，因为它本身即是一个优化库。然而，在SLAM中更多的问题是，一个带有许多个相机位姿和许多个空间点的优化问题如何求解。特别地，当相机位姿以李代数表示时，误差项关于相机位姿的导数如何计算，将是一件值得详细讨论的事。我们将在后续内容中发现，g2o提供了大量现成的顶点和边，非常便于相机位姿估计问题。而在Ceres中，我们不得不自己实现每一个Cost Function，有一些不便。
在实践部分的两个程序中，我们没有去计算曲线模型关于三个参数的导数，而是利用了优化库的数值求导，这使得理论和代码都更简洁。Ceres库提供了基于模板元的自动求导和运行时的数值求导，而g2o只提供了运行时数值求导这一种方式。但是，对于大多数问题，如果能够推导出雅可比矩阵的解析形式并告诉优化库，就可以避免数值求导中的诸多问题。
最后，希望读者能够适应Ceres和g2o这些大量使用模板编程的方式。也许一开始会看上去比较吓人（特别是Ceres设置残差块的括号运算符，以及g2o初始化部分的代码），但是熟悉之后，就会觉得这样的方式是自然的，而且容易扩展。我们将在SLAM后端一讲中继续讨论稀疏性、核函数、位姿图等问题。

课后习题

有参考CSDN博主木木夕v的博客
1. 证明线性方程 $A x = b$ 当系数矩阵A 超定时，最小二乘解为 $x=\left ( A^{T}A \right )^{-1}A^{T}b$

目标函数 = $\sum(y^{*} - y)^2$

令最小二乘的目标函数为： $F(x) = (Ax - b)^T(Ax - b)$

$F(x) = x^TA^TAx - x^TA^Tb - b^TAx + b^Tb$

对 $x$ 求导： $F^{'}(x) = 2A^TAx - 2A^Tb$

令 $F^{'}(x)=0$ ，得： $x = (A^TA)^{-1}A^Tb$

矩阵求导公式：

$①\frac{\partial (x^Ta)}{\partial x} = \frac{\partial (a^Tx)}{\partial x} = a.$ 其中, $a$ 为常数向量， $a = (a_1,a_2,···,a_n)^T.$

$②\frac{\partial (x^Tx)}{\partial x} = 2x.$

$③\frac{\partial (x^TAx)}{\partial x} = Ax + A^Tx.$ 其中， $A_{n×n}$ 是常数矩阵， $A_{n×n} = \sum\limits_{i = 1}^n\sum\limits_{j = 1}^na_{ij}$

$④\frac{\partial (a^Txx^Tb)}{\partial x} = ab^Tx + ba^Tx.$ 其中， $a, b$ 为常数向量， $a = (a_1,a_2,···,a_n)^T,b=(b_1,b_2,···,b_n)^T.$

2. 调研最速下降法、牛顿法、高斯牛顿法和列文伯格—马夸尔特方法各有什么优缺点？除了我们举的 Ceres 库和 g2o 库，还有哪些常用的优化库。（你可能会找到一些MATLAB上的库）

最速下降法（一阶导数）
优点：在前几次迭代中目标函数下降较快。
缺点：过于贪心，容易走出锯齿路线，反而增加迭代次数，导致局部收敛速率下降。
牛顿法（二阶导数）
优点：较为直观，收敛较快，迭代次数较少，解决了最速下降法一阶导数为0
缺点：需要计算二阶导数 $H$ 矩阵，当问题规模较大时，该矩阵的计算难度较大，迭代时间较长。
高斯牛顿法（雅可比矩阵）
优点：使用 $J(x)J^T(x)$ 作为 $H (x)$ 的近似，避免了二阶求导。
缺点：实际中该近似只有半正定性，可能出现了 $JJ^T$ 为奇异矩阵或病态的情况，此时增量的稳定性较差，导致算法不收敛
列文伯格——马夸尔特方法：（信赖区域）
优点：在高斯牛顿法中引入阻尼项——信赖区域，可以在一定程度上避免线性方程组的系数矩阵奇异或者病态问题。
缺点：收敛速度比高斯牛顿法慢。

还有的优化库：Ipopt、NLopt、slam++、Optimization Toolbox（MATLAB）
3. 为什么高斯牛顿法的增量方程系数矩阵可能不正定？不正定有什么几何含义？为什么在这种情况下解就不稳定了？
高斯牛顿法将 $JJ^T$ 作为Hessian矩阵的近似，而在实际的计算中 $JJ^T$ 只有半正定性（例如：当J为零向量时，增量方程系数矩阵是不正定的）。当Hessian矩阵不正定时，此时的目标函数 $f(x_0+\Delta x)≈f(x_0)$ ，此时对于 $f (x)$ 的近似是一条水平的直线，在 $x_0$ 点处函数是“平坦的”，无法给出下一次迭代的下降方向（一阶导数恒为0），所以在这种情况下解不稳定。
4. DogLeg是什么？它与高斯牛顿法和列文伯格—马夸尔特方法有何异同？请搜索相关的材料。
DogLeg是信赖区域（Trust-Region）方法的一种，包含了高斯牛顿法和最速下降法两种方法，即用置信域的方法在最速下降法和高斯牛顿法之间进行切换，相当于是一种加权求解。通过引入信赖区域的概念，对两种方法计算得到的增量，在信赖区域进行加权得到新的增量并用于后续的迭代，因此计算得到的下降路径类似于DogLeg，所以被称为DogLeg方法。

DogLeg包含了高斯牛顿法，当高斯牛顿法计算得到的增量在信赖区域时，DogLeg较为接近高斯牛顿法；不同的是DogLeg还包含了最速下降法，使得下降搜索方向和增量计算更加合理，算法更加鲁棒。
列文伯格-马夸尔特方法与DogLeg都引入了信赖区域的概念，都结合了高斯牛顿法和列文伯格-马夸尔特方法；不同的是DogLeg需要计算更多的中间量，但是在当前步长不满足约束时，DogLeg不需要重新计算Hessian矩阵。

5. 阅读 Ceres 的教学材料（http://ceres-solver.org/tutorial.html）以更好地掌握其用法
略
6. 阅读 g2o 自带的文档，你能看懂它吗？如果还不能完全看懂，请在阅读第10讲和第11讲之后再回来看。
略
7. *请更改曲线拟合实验中的曲线模型，并用 Ceres 和 g2o 进行优化实验。例如，可以使用更多的参数和更复杂的模型。
用到回来再更。