SLAM 第六讲 非线性优化

1、状态估计问题

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

经典 SLAM 模型由一个运动方程和一个观测方程构成：

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

xk是相机的位姿变量，可以由Tk∈SE(3)表达。运动方程与输入的具体形式有关，在视觉SLAM中没有特殊性（和普通的机器人、车辆的情况一样）。观测方程则由针孔模型给定。假设在xk处对路标yj进行了一次观测，对应到图像上的像素位置zk,j，那么，观测方程可以表示成

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

其中K为相机内参，s为像素点的距离，也是(Rkyj +tk)的第三个分量。如果使用变换矩阵Tk描述位姿，那么路标点yj必须以齐次坐标来描述，计算完成后要转换为非齐次坐标。

在运动和观测方程中，通常假设两个噪声项wk,vk,j满足零均值的高斯分布：

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

其中N表示高斯分布，0表示零均值，Rk,Qk,j为协方差矩阵。在这些噪声的影响下，希望通过带噪声的数据z和u推断位姿x和地图y（以及它们的概率分布），这构成了一个状态估计问题。

处理这个状态估计问题的方法分成两种。

第一种：由于在SLAM过程中，这些数据是随时间逐渐过来的，所以在直观上，应该持有一个当前时刻的估计状态，然后用新的数据来更新它。这种方式称为增量（incremental）的方法，或者叫滤波器。在SLAM的早期研究，主要使用扩展卡尔曼滤波器（EKF）及其衍生方法来求解。

第二种：是把数据累加起来一并处理，这种方式称为批量（batch）的方法。例如，可以把0到k时刻所有的输入和观测数据都放在一起，求在这样的输入和观测下，如何估计整个0到k时刻的轨迹与地图？

增量方法仅关心当前时刻的状态估计xk，而对之前的状态则不多考虑；相对地，批量方法可以在更大的范围达到最优化，被认为优于传统的滤波器，成为当前视觉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的条件概率分布：

$$
P(x,y|z,u)
$$

特别地，当不知道控制输入，只有一张张图像时，即只考虑观测方程带来的数据时，相当于估计P(x,y|z)的条件概率分布，此问题也称为Structure from Motion（SfM），即如何从许多图像中重建三维空间结构。

为了估计状态变量的条件分布，利用贝叶斯法则，有：

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

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

$$
(x, y)^*_{Map} = argmax P (x,y| z,u) = argmax P(z,u|x,y)P(x,y)
$$

贝叶斯法则的分母部分与待估计的状态x,y无关，因而可以忽略。贝叶斯法则说明，求解最大后验概率等价于最大化似然和先验的乘积。进一步，如果不知道机器人位姿或路标大概在什么地方，此时就没有了先验。那么，可以求解最大似然估计（Maximize Likelihood Estimation，MLE）：

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

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

1.2 最小二乘的引出

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

$$
z_{k,j} = h(y_i, x_k) + v(k,j)
$$

假设噪声项vk ∼ N (0,Qk,j)，观测数据的条件概率为：

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

它依然是一个高斯分布。考虑单次观测的最大似然估计，可以使用最小化负对数来求一个高斯分布的最大似然。

高斯分布在负对数下有较好的数学形式。考虑任意高维高斯分布x ∼ N(µ,Σ)，它的概率密度函数展开形式为：

$$
P(x) = \frac{1}{\sqrt[]{ (2\pi)^N det(\Sigma)}}
\exp \big( -\frac{1}{2}(x-\mu)^T \Sigma^{-1}(x-\mu) \big)
$$

对其取负对数，则变为：

$$
-\ln(P(x))= \frac{1}{2} \ln \big({ (2\pi)^N det(\Sigma)} \big)

-\frac{1}{2}(x-\mu)^T \Sigma^{-1}(x-\mu)
$$

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

$$
b
$$

该式等价于最小化噪声项（即误差）的一个二次型。这个二次型称为马哈拉诺比斯距离（Mahalanobis distance），又叫马氏距离。它也可以看成是由(Qk,j)-1 加权之后的欧氏距离（二范数），这里(Qk,j)-1也叫做信息矩阵，即高斯分布协方差矩阵之逆。

现在考虑批量时刻的数据。通常假设各个时刻的输入和观测是相互独立的，这意味着各个输入之间是独立的，各个观测之间是独立的，并且输入和观测也是独立的。于是可以对联合分布进行因式分解：

$$
b
$$

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

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

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

$$
\min J(x,y) = \sum_{k} e_{u,k}^T R_{k}^{-1} e_{u,k} +
\sum_{k}\sum_{j} e_{z,k,j}^T R_{k,j}^{-1} e_{z,k,j}
$$

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

SLAM 中的最小二乘问题具有一些特定的结构：

• 整个问题的目标函数由许多个误差的（加权的）二次型组成。虽然总体的状态变量维数很高，但每个误差项都是简单的，仅与一两个状态变量有关。例如，运动误差只与$x_{k−1}, x_k$ 有关，观测误差只与$x_k,y_j$有关。这种关系会让整个问题有一种稀疏的矩阵形式，计算量大大减少。

• 如果使用李代数表示增量，则该问题是无约束的最小二乘问题。但如果用旋转矩阵/变换矩阵描述位姿，则会引入旋转矩阵自身的约束，即需在问题中加入$R^TR = I$且$det(R) =1$的条件。额外的约束会使优化变得更困难。这体现了李代数的优势。

• 使用了二次型度量误差。误差的分布将影响此项在整个问题中的权重。例如，某次的观测非常准确，那么协方差矩阵就会“小”，而信息矩阵就会“大”，所以这个误差项会在整个问题中占有较高的权重。

1.3 例子：批量状态估计

考虑一个离散时间系统：

$$
x_k = x_{k-1} + u_k + w_k, \quad w_k \sim N(0, Q_k) \
z_k = x_{k} + n_k, \qquad\qquad n_k \sim N(0, R_k)
$$

这可以表达一辆沿 x 轴前进或后退的汽车。第一个公式为运动方程，uk 为输入，wk 为噪声；第二个公式为观测方程，zk 为对汽车位置的测量。取时间 k = 1,…,3，现希望根据已有的 v,y 进行状态估计。设初始状态 x0 已知，来推导批量（batch）状态的最大似然估计。

首先，令批量状态变量为$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$。最大似然估计为：

$$
x^*{map} = \argmax P(x|u,z) = \argmax P(u,z|x)\
\quad
= \sum^{3}{k=1} (u_k| x_{k-1, x_k})
\sum^{3}_{k=1} (z_k| )
$$

运动方程：

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

观测方程：

$$
P(z_k, x_k) = N(x_k, R_k)
$$

构建误差变量：

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

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

$$
\min \sum_{k=1}^3 e_{u,k}^T Q_k^{-1} e_{u,k} +
\sum_{k=1}^3 e_{z,k}^T R_{k}^{-1} e_{z,k}
$$

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

$$
y - Hx = e \sim N(0, \Sigma)
$$

那么：

$$
H = []

\
\Sigma = diag(Q_1, Q_2, Q_3, R_1, R_2, R_3)
$$

整个问题可以写成：

$$
x_{map}^* = \argmax e^T \Sigma^{-1}e.
$$

这个问题有唯一的解：

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

2、非线性最小二乘

考虑一个最小二乘问题：

$$
\min_{x} F(x) = \frac{1}{2} ||f(x)||^2_2
$$

其中，自变量$x\in R_n$，f是任意标量非线性函数f(x):Rn->R。注意这里的系数1/2是无关紧要的。如何求解这样一个优化问题：如果f是个数学形式上很简单的函数，那么该问题可以用解析形式来求。令目标函数的导数为零，然后求解x的最优值，就求二元函数的极值一样：

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

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

给定某个初始值x0。
对于第 k 次迭代，寻找一个增量 ∆xk，使得

$$
|| f(x_k + \Delta x_k) ||^2_2
$$

达到极小值。

若 ∆xk 足够小，则停止。
否则，令x(k+1) = xk + ∆xk，返回第 2 步。

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

下面是如何寻找这个增量 ∆xk的方法。

2.1 一阶和二阶梯度法

考虑第 k 次迭代，假设在xk处，想要找到增量 ∆xk，最直观的方式是将目标函数在xk附近进行泰勒展开：

$$
F
$$

其中J(xk) 是 F(x)关于x的一阶导数（也叫梯度、雅可比矩阵﹝Jacobian﹞），H 则是二阶导数（海塞﹝Hessian﹞矩阵），它们都在xk 处取值。可以选择保留泰勒展开的一阶或二阶项，那么对应的求解方法则称为一阶梯度或二阶梯度法。如果保留一阶梯度，取增量为反向的梯度，即可保证函数下降：∆x∗= −J(xk)

这只是个方向，通常还要再指定一个步长 λ。步长可以根据一定的条件来计算，在机器学习当中也有一些经验性质的方法。这种方法被称为最速下降法。它的直观意义是：只要沿着反向梯度方向前进，在一阶（线性）的近似下，目标函数必定会下降。

以上都是在第k次迭代时进行的，并不涉及其他的迭代信息。为了简化符号，省略下标k，并认为这些对任意一次迭代都成立。

选择保留二阶梯度信息，此时增量方程为：

$$
$$

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

$$
求解这个线性方程就得到了增量。此类方法又称为牛顿法。

一阶和二阶梯度法都十分直观，只要把函数在迭代点附近进行泰勒展开，并针对更新量做最小化即可。用一个一次或二次的函数近似原函数，然后用近似函数的最小值来估计原函数的极小值。只要原目标函数局部看起来像一次或二次函数，这类算法就是成立的。不过这两种方法也存在问题：最速下降法过于贪心，容易走出锯齿路线，反而增加了迭代次数。而牛顿法则需要计算目标函数的H矩阵，这在问题规模较大时非常困难，通常倾向于避免H的计算。所以引出了下面的一些方法。

2.2 高斯牛顿法

牛顿法是对目标函数 F(x) 进行一阶泰勒展开，而高斯牛顿法是对f(x)进行一阶泰勒展开：f (x + ∆x) ≈ f(x) + J(x)T∆x.

注：把 J(x) 写成列向量，那么它可以和 ∆x 进行内积，得到一个标量。

这里J(x)T为f(x)关于x的导数，为n×1的列向量。目标是寻找增量∆x，使得∥f(x+∆x)∥2达到最小。为了求 ∆x，需要解一个线性的最小二乘问题：

F
$$

根据极值条件，将上述目标函数对 ∆x求导，并令导数为零。可以得到如下方程组：

$$
F
$$

这个方程是关于变量∆x的线性方程组，称它为增量方程，也可以称为高斯牛顿方程（GaussNewton equation）或者正规方程（Normal equation）。把左边的系数定义为H，右边定义为g，那么上式变为：H∆x = g.

对比牛顿法可见，高斯牛顿法用JJT作为牛顿法中二阶Hessian矩阵的近似，从而省略了计算H的过程。求解增量方程是整个优化问题的核心所在。高斯牛顿法的算法步骤可以写成：

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

从算法步骤中，主要还是增量的求解。只要能够顺利解出增量，就能保证目标函数能够正确地下降。

为了求解增量方程，需要求解H−1，这需要H矩阵可逆，但实际数据中计算得到的JJT只有半正定性。也就是说，在使用高斯牛顿法时，可能出现JJT为奇异矩阵或者病态（ill-condition）的情况，此时增量的稳定性较差，导致算法不收敛。直观地说，原函数在这个点的局部近似不像一个二次函数。假设H非奇异也非病态，如果求出来的步长∆x太大，也会导致采用的局部近似式：f (x + ∆x) ≈ f(x) + J(x)T∆x.不够准确，这样一来无法保证它的迭代收敛。

在非线性优化领域，相当多的算法都可以归结为高斯牛顿法的变种。这些算法都借助了高斯牛顿法的思想并且通过改进修正其缺点。例如一些线搜索方法 (line search method) 加入了一个步长α，在确定了∆x后进一步找到 α 使得 ∥f(x + α∆x)∥2 达到最小，而不是简单地令 α = 1。

列文伯格—马夸尔特方法在一定程度上修正了这些问题，比高斯牛顿法更为鲁棒，但收敛速度会比高斯牛顿法更慢，被称为阻尼牛顿法（Damped Newton Method）。

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

高斯牛顿法中采用的近似二阶泰勒展开只能在展开点附近有较好的近似效果，所以应该给∆x添加一个范围，称为信赖区域（Trust Region）。这个范围定义了在什么情况下二阶近似是有效的，这类方法也称为信赖区域方法（Trust Region Method）。在信赖区域里边，近似是有效的；出了这个区域，近似可能会出问题。

那么如何确定这个信赖区域的范围呢？一个比较好的方法是根据近似模型跟实际函数之间的差异来确定：如果差异小，说明近似效果好，扩大近似的范围；反之，如果差异大，就缩小近似的范围。我们定义一个指标 ρ 来刻画近似的好坏程度：

$$
F
$$

ρ 的分子是实际函数下降的值，分母是近似模型下降的值。如果 ρ 接近于 1，则近似是好的。如果 ρ 太小，说明实际减小的值远少于近似减小的值，则认为近似比较差，需要缩小近似范围。反之，如果 ρ 比较大，则说明实际下降的比预计的更大，可以放大近似范围。

于是构建一个改良版的非线性优化框架，该框架会比高斯牛顿法有更好的效果：

给定初始值 x0，以及初始优化半径 µ。
对于第 k 次迭代，在高斯牛顿法的基础上加上信赖区域，求解：

$$
F
$$

其中 µ 是信赖区域的半径，D 为系数矩阵。
计算 ρ。
若 ρ > 3/4，则设置 µ = 2µ。
若 ρ < 1/4，则设置 µ = 0.5µ。
如果 ρ 大于某阈值，则认为近似可行。令 xk+1 = xk + ∆xk。
判断算法是否收敛。如不收敛则返回第 2 步，否则结束。

这里近似范围扩大的倍数和阈值都是经验值，可以替换成别的数值。

$$
F
$$

这个式子中，把增量限定于一个半径为 µ 的球中，认为只在这个球内才是有效的。带上D之后，这个球可以看成一个椭球。在列文伯格提出的优化方法中，把D取成单位阵I，相当于直接把 ∆xk约束在一个球中。随后，马夸尔特提出将D取成非负数对角阵——实际中通常用JTJ 的对角元素平方根，使得在梯度小的维度上约束范围更大一些。

在列文伯格—马夸尔特优化中，需要求解这个子问题来获得梯度。

这个子问题是带不等式约束的优化问题，用拉格朗日乘子把约束项放到目标函数中，构成拉格朗日函数：

$$
F
$$

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

$$
F
$$

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

(H + λI)∆xk = g.

当参数λ比较小时，H占主要地位，这说明二次近似模型在该范围内是比较好的，列文伯格—马夸尔特方法更接近于高斯牛顿法。另一方面，当λ比较大时，λI占据主要地位，列文伯格—马夸尔特方法更接近于一阶梯度下降法（即最速下降），这说明附近的二次近似不够好。列文伯格—马夸尔特方法的求解方式，可在一定程度上避免线性方程组的系数矩阵的非奇异和病态问题，提供更稳定、更准确的增量 ∆x。

在实际中，还存在许多其他的方式来求解增量，例如

[31] J. Nocedal and S. Wright, Numerical Optimization. Springer Science & Business Media, 2006.

实际问题中，通常选择高斯牛顿法或列文伯格—马夸尔特方法其中之一作为梯度下降策略。当问题性质较好时，用高斯牛顿。如果问题接近病态，则用列文伯格—马夸尔特方法。

梯度下降法	场景
牛顿法
高斯-牛顿	问题质量较好
列文伯格-马夸尔特方法	问题接近病态？

小结

专门介绍数值优化的书籍

[31] J. Nocedal and S. Wright, Numerical Optimization. Springer Science & Business Media, 2006.

以高斯牛顿法和列文伯格—马夸尔特方法为代表的优化方法，在很多开源的优化库中都已经实现并提供给用户。最优化是处理许多实际问题的基本数学工具，不光在视觉 SLAM 中起着核心作用，在类似于深度学习等其他领域，它也是求解问题的核心方法之一（深度学习数据量很大，以一阶方法为主）。

无论是高斯牛顿法还是列文伯格—马夸尔特方法，在做最优化计算时，都需要提供变量的初始值。这个初始值不能随意设置。实际上非线性优化的所有迭代求解方案，都需要用户来提供一个良好的初始值。由于目标函数太复杂，导致在求解空间上的变化难以预测，对问题提供不同的初始值往往会导致不同的计算结果。这种情况是非线性优化的通病：大多数算法都容易陷入局部极小值。因此，无论是哪类科学问题，提供初始值都应该有科学依据，例如视觉 SLAM 问题中，会用 ICP、PnP 之类的算法提供优化初始值（视觉前端）。一个良好的初始值对最优化问题非常重要。

如何求解线性增量方程组呢？在视觉SLAM算法里，经常遇到∆x的维度很大，如果是要做大规模的视觉三维重建，就会经常发现这个维度可以轻易达到几十万甚至更高的级别。要对那么大个矩阵进行求逆是大多数处理器无法负担的，因此存在着许多针对线性方程组的数值求解方法。在不同的领域有不同的求解方式，但几乎没有一种方式是直接求系数矩阵的逆，会采用矩阵分解的方法来解线性方程，例如 QR、Cholesky 等分解方法。(工程数学)

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

3 实践：曲线拟合问题

3.1手写高斯牛顿法

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

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

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

$$
\min_{a,b,c} 1/2 \sum_{i=1}^N
|| y_i - \exp( ax_i^2 + bx_i +c ) || ^2
$$

在这个问题中，待估计的变量是 a,b,c，而不是 *x。程序里先根据模型生成 *x,y 的真值，然后在真值中添加高斯分布的噪声。随后，使用高斯牛顿法来从带噪声的数据（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)\
\frac{\partial e_i} {\partial b} = -x_i \exp(ax_i^2 + bx_i + c)\
\frac{\partial e_i} {\partial c} = - \exp(ax_i^2 + bx_i + c) \
$$

于是

$$
J_i = [\frac{\partial e_i}{\partial a}, \frac{\partial e_i}{\partial b}, \frac{\partial e_i}{\partial c}]
$$

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

$$
\big( \sum_{i=1}{100} J_i(\sigma^2)^{-1} J_i^T \big)
\Delta x_k =
\sum_{i=1}^{100} - J_i(\sigma^2)^-1 e_i

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

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

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

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

3.2使用 Ceres 进行曲线拟合

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

$$
\min_{x} 1/2 \sum_{j} p_i (|| f_i (x_{i_1}, …, x_{i_n}) ||^2)
\
s.t. \qquad l_j \leq x_j \leq u_j.

在这个问题中，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 地址为：ceres-solver/ceres-solver，

安装依赖项

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


sudo apt install liblapack-dev libsuitesparse-dev libcxsparse3.1.4 \
        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() 方法。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 之外的各种问题。

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

代码实现

3.3使用 g2o 进行曲线拟合

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

图优化理论简介

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

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

如下图：

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

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

g2o 的编译与安装

安装依赖：

1	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会给出优化的结果。

第一版书代码Debug：

《视觉SLAM十四讲》 g2o实践代码报错解决方法（第六讲为例） - 代码先锋网

4 小结

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

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

非线性拟合（曲线拟合）

非线性优化梯度下降法
高斯牛顿法
列文伯格-马夸尔特法

工具库	特性	求导	使用
Ceres	-	（自动求导）基于模板元的自动求导和运行时的数值求导
G2o	提供了大量现成的顶点和边，非常便于相机位姿估计问题	运行时数值求导这一种方式	必须先把问题转换为图优化，定义新的顶点和边