《视觉SLAM十四讲》读书笔记（四）

6.1 状态估计问题

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

考虑从$1$到$N$的所有时刻，并假设有$M$个路标点。定义所有时刻的机器人位姿和路标点坐标为
$$\mathbf{x}=\left\{{\mathbf{x}}_1,\dots ,{\mathbf{x}}_N\right\},\mathbf{y}=\left\{{\mathbf{y}}_1,\dots ,{\mathbf{y}}_M\right\}$$
同样地, 用不带下标的 $\mathbf{u}$ 表示所有时刻的输人, $\mathbf{z}$ 表示所有时刻的观测数据。 对机 器人状态的估计, 就是已知输人数据 $\mathbf{u}$ 和观测数据 $\mathbf{z}$ 的条件下, 求状态 $\mathbf{x},\mathbf{y}$ 的条件概率分布:
$$P(\mathbf{x},\mathbf{y}\mid \mathbf{z},\mathbf{u})\text{\hspace{1em}(6.4)}$$
特别地, 当我们不知道控制输人, 只有一张张的图像时, 即只考虑观测方程带来的数据时, 相当于估计 $P(\mathbf{x},\mathbf{y}\mid \mathbf{z})$ 的条件概率分布, 此问题也称为 $\mathrm{S}\mathrm{f}\mathrm{M}$ , 即如何从许多图像中重建三维空间结构。为了估计状态变量的条件分布, 利用贝叶斯法则, 有
$$P(\mathbf{x},\mathbf{y}\mid \mathbf{z},\mathbf{u})=\frac{P(\mathbf{z},\mathbf{u}\mid \mathbf{x},\mathbf{y})P(\mathbf{x},\mathbf{y})}{P(\mathbf{z},\mathbf{u})}\propto \underset{\text{似然 }}{P(\mathbf{z},\mathbf{u}\mid \mathbf{x},\mathbf{y})}\underset{\text{先验 }}{P(\mathbf{x},\mathbf{y})}.\text{\hspace{1em}(6.5)}$$
贝叶斯法则左侧称为后验概率，右侧的$P(\mathbf{z}\mid \mathbf{x})$称为似然 (Likehood)，另一部分$P(\mathbf{x})$称为先验 (Prior)。

注：式(6.5)的一个重要意义是，当先验概率确定时，后验概率与似然成正比。

直接求后验分布是困难的，但是求一个状态最有估计，使得在该状态下后验概率最大化，则是可行的：
$$(\mathbf{x},\mathbf{y}{)}_{\text{MAP }}^{\ast }=\arg \max P(\mathbf{x},\mathbf{y}\mid \mathbf{z},\mathbf{u})=\arg \max P(\mathbf{z},\mathbf{u}\mid \mathbf{x},\mathbf{y})P(\mathbf{x},\mathbf{y})\text{\hspace{1em}(6.6)}$$
贝叶斯法则的分母部分与待估计的状态$\mathbf{x},\mathbf{y}$无关，因而可以忽略。贝叶斯法则告诉我们，求解最大后验概率等价于最大化似然与先验的乘积。如果我们不知道机器人位姿或路标大概在在什么地方（预测），此时就没有了先验。那么，可以求解最大似然估计：
$$(\mathbf{x},\mathbf{y}{)}_{\mathrm{M}\mathrm{L}\mathrm{E}}^{\ast }=\arg \max P(\mathbf{z},\mathbf{u}\mid \mathbf{x},\mathbf{y})\text{\hspace{1em}(6.7)}$$
通过贝叶斯法则，我们的求解目标由“基于现在观测到的数据，机器人最可能处于什么样的状态”转换成“在什么样的状态下，最可能产生现在观测到的数据”。

6.1.2 最小二乘的引出

SLAM的观测模型：
$${\mathbf{z}}_{k,j}=h\left({\mathbf{y}}_j,{\mathbf{x}}_k\right)+{\mathbf{v}}_{k,j}$$
由于我们假设了噪声项 ${\mathbf{v}}_k\sim \mathcal{N}\left(0,{\mathbf{Q}}_{k,j}\right)$ , 所以观测数据的条件概率为
$$P\left({\mathbf{z}}_{j,k}\mid {\mathbf{x}}_k,{\mathbf{y}}_j\right)=N\left(h\left({\mathbf{y}}_j,{\mathbf{x}}_k\right),{\mathbf{Q}}_{k,j}\right)$$
它依然是一个高斯分布。考虑单次观测的最大似然估计, 可以使用最小化负对数来求一个高斯分布的最大似然。

我们知道高斯分布在负对数下有较好的数学形式。考虑任意高维高斯分布 $x\sim \mathcal{N}(\mu ,\Sigma )$ , 它 的概率密度函数展开形式为
$$P(\mathbf{x})=\frac{1}{\sqrt{(2\pi {)}^N\det (\mathbf{\Sigma })}}\exp \left(-\frac{1}{2}(\mathbf{x}-\mathbf{\mu }{)}^{\mathrm{T}}{\mathbf{\Sigma }}^{-1}(\mathbf{x}-\mathbf{\mu })\right)\text{\hspace{1em}(6.8)}$$
对其取负对数, 则变为
$$-\ln (P(\mathbf{x}))=\frac{1}{2}\ln \left((2\pi {)}^N\det (\mathbf{\Sigma })\right)+\frac{1}{2}(\mathbf{x}-\mathbf{\mu }{)}^{\mathrm{T}}{\mathbf{\Sigma }}^{-1}(\mathbf{x}-\mathbf{\mu })\text{\hspace{1em}(6.9)}$$
因为对数函数时单调递增的，所有对原函数求最大化相当于对负对数求最小化。在最小化上式的$\mathbf{x}$时，第一项与$\mathbf{x}$无关，可以略去。于是，只要最小化右侧的二次型项，就得到了对状态的最大似然估计。代入SLAM的观测模型，相当于在求：
$$\begin{array}{rl}{\left({\mathbf{x}}_k,{\mathbf{y}}_j\right)}^{\ast }& =\arg \max \mathcal{N}\left(h\left({\mathbf{y}}_j,{\mathbf{x}}_k\right),{\mathbf{Q}}_{k,j}\right)\\ & =\arg \min \left({\left({\mathbf{z}}_{k,j}-h\left({\mathbf{x}}_k,{\mathbf{y}}_j\right)\right)}^{\mathrm{T}}{\mathbf{Q}}_{k,j}^{-1}\left({\mathbf{z}}_{k,j}-h\left({\mathbf{x}}_k,{\mathbf{y}}_j\right)\right)\right)\end{array}\text{\hspace{1em}(6.10)}$$
该式等价于最小化噪声项（即误差）的马氏距离。

现在我们考虑批量时刻的数据。通常假设各个时刻的输入和观测是相互独立的，这意味着各个输入之间的独立的，各个观测之间是独立的，并且输入和观测也是独立的。于是我们可以对联合分布进行因式分解：
$$P(\mathbf{z},\mathbf{u}\mid \mathbf{x},\mathbf{y})=\prod _{k}P\left({\mathbf{u}}_k\mid {\mathbf{x}}_{k-1},{\mathbf{x}}_k\right)\prod _{k,j}P\left({\mathbf{z}}_{k,j}\mid {\mathbf{x}}_k,{\mathbf{y}}_j\right)\text{\hspace{1em}(6.11)}$$
这说明我们可以独立地处理各时刻的运动和观测。定义各次输人和观测数据与模型之间的误差:
$$\begin{array}{rl}{e}_{\mathbf{u},k}& ={\mathbf{x}}_k-f\left({\mathbf{x}}_{k-1},{\mathbf{u}}_k\right)\\ e_{\mathbf{z},j,k}& ={\mathbf{z}}_{k,j}-h\left({\mathbf{x}}_k,{\mathbf{y}}_j\right)\end{array}\text{\hspace{1em}(6.12)}$$
那么, 最小化所有时刻估计值与真实读数之间的马氏距离, 等价于求最大似然估计。负对数允许我们把乘积变成求和:
$$\min J(\mathbf{x},\mathbf{y})=\sum _k{e}_{\mathbf{u},k}^{\mathrm{T}}{\mathbf{R}}_k^{-1}{\mathbf{e}}_{\mathbf{u},k}+\sum _k\sum _j{e}_{\mathbf{z},k,j}^{\mathrm{T}}{\mathbf{Q}}_{k,j}^{-1}{e}_{\mathbf{z},k,j}\text{\hspace{1em}(6.13)}$$
这样就得到了一个最小二乘问题，它的解等价于状态的最大似然估计。

6.1.3 最小二乘问题解析解

最小二乘的目标函数为
$$\min \sum _{k=1}^3{e}_{\mathbf{u},k}^{\mathrm{T}}{\mathbf{Q}}_k^{-1}{e}_{\mathbf{u},k}+\sum _{k=1}^3{e}_{\mathbf{z},k}^{\mathrm{T}}{\mathbf{R}}_k^{-1}{\mathbf{e}}_{z,k}$$
如果系统是线性系统，那么我们可以很容易地将它写成向量形式，定义向量$\mathrm{y}=[\mathrm{u},\mathrm{z}{]}^T$，那么可以写出矩阵$\mathrm{H}$，使得
$$y-Hx=e\sim \mathcal{N}(0,\Sigma )$$
那么:
$$\mathbf{H}=\left[\begin{array}{cccc}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{array}\right]$$
且 $\mathbf{\Sigma }=\mathrm{diag}\left({\mathbf{Q}}_1,{\mathbf{Q}}_2,{\mathbf{Q}}_3,{\mathbf{R}}_1,{\mathbf{R}}_2,{\mathbf{R}}_3\right)$ 。整个问题可以写成
$$x_{\mathrm{m}\mathrm{a}\mathrm{p}}^{\ast }=\arg \min e^{\mathrm{T}}{\Sigma }^{-1}e$$
之后我们将看到, 这个问题有唯一的解:
$${\mathbf{x}}_{\mathrm{m}\mathrm{a}\mathrm{p}}^{\ast }={\left({\mathbf{H}}^{\mathrm{T}}{\mathbf{\Sigma }}^{-1}\mathbf{H}\right)}^{-1}{\mathbf{H}}^{\mathrm{T}}{\mathbf{\Sigma }}^{-1}\mathbf{y}$$

6.2 非线性最小二乘

最小二乘问题的基本形式：
$$\underset{\mathbf{x}}{\min }F(\mathbf{x})=\frac{1}{2}\Vert f(\mathbf{x}){\Vert }_2^2\text{\hspace{1em}(6.24)}$$
迭代求解过程：

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

6.2.1 一阶和二阶梯度法

将目标方程在$x_k$附近进行泰勒展开：
$$F\left({\mathbf{x}}_k+\Delta {\mathbf{x}}_k\right)\approx F\left({\mathbf{x}}_k\right)+\mathbf{J}{\left({\mathbf{x}}_k\right)}^{\mathrm{T}}\Delta {\mathbf{x}}_k+\frac{1}{2}\Delta {\mathbf{x}}_k^{\mathrm{T}}\mathbf{H}\left({\mathbf{x}}_k\right)\Delta {\mathbf{x}}_k\text{\hspace{1em}(6.26)}$$
其中 $\mathbf{J}\left({\mathbf{x}}_k\right)$ 是 $F(\mathbf{x})$ 关于 $\mathbf{x}$ 的一阶导数 [也叫梯度、雅可比 ( Jacobian ) 矩阵]，$\mathbf{H}$ 则是二阶 导数 [海塞 (Hessian ) 矩阵]，它们都在 $x_k$ 处取值。

（1）最速下降法

如果保留一阶梯度，那么取增量为反向的梯度，即可保证函数下降:
$$\Delta x^{\ast }=-J\left(x_k\right)\text{\hspace{1em}(6.27)}$$
当然这只是个方向，通常我们还要再指定一个步长$\lambda$。

（2）牛顿法

我们可以对式(6.26)保留二阶梯度信息，此时增量方程味：
$$\Delta {\mathbf{x}}^{\ast }=\arg \min \left(F(\mathbf{x})+\mathbf{J}(\mathbf{x}{)}^{\mathrm{T}}\Delta \mathbf{x}+\frac{1}{2}\Delta {\mathbf{x}}^{\mathrm{T}}\mathbf{H}\Delta \mathbf{x}\right)\text{\hspace{1em}(6.28)}$$
右侧只含 $\Delta x$ 的零次、一次和二次项。求右侧等式关于 $\Delta x$ 的导数并令它为零，得到
$$J+H\Delta x=0\Rightarrow H\Delta x=-J\text{\hspace{1em}(6.29)}$$
求解这个$\Delta x=-H^{-1}J$即可得到增量。

（3）小结 1

1. 最速下降法过于贪心，容易走出锯齿路线，反而增加了迭代次数。
2. 牛顿法需要计算目标函数的$H$矩阵，这在问题规模较大时非常困难，我们通常倾向于避免$H$的计算。

---

（4）补充：矩阵求导公式

若 $f(\mathbf{X})和g(\mathbf{X})$ 分别是矩阵 $\mathbf{X}$ 的实值函数, $c_1,c_2$ 为实常数, 则

• 线性法则 :
  $$\frac{\partial \left[c_{1}f(\mathbf{X})+c_{2}g(\mathbf{X})\right]}{\partial \mathbf{X}}=c_1\frac{\partial f}{\partial \mathbf{X}}+c_2\frac{\partial g}{\partial \mathbf{X}}$$ .
• 乘积法则: $$\frac{\partial [f(\mathbf{X})g(\mathbf{X})]}{\partial \mathbf{X}}=g(\mathbf{X})\frac{\partial f}{\partial \mathbf{X}}+f(\mathbf{X})\frac{\partial g}{\partial \mathbf{X}}$$ .
• 商法则 : $$\frac{\partial [f(\mathbf{X})/g(\mathbf{X})]}{\partial \mathbf{X}}=\frac{1}{g^2(\mathbf{X})}\left[g(\mathbf{X})\frac{\partial f}{\partial \mathbf{X}}-f(\mathbf{X})\frac{\partial g}{\partial \mathbf{X}}\right]$$.
• 链式法则: 若 $y=f(\mathbf{X}),g(y)$ 分别为矩阵 $\mathbf{X}$ 和标量 $y$ 的实值函数, 则
  $$\frac{\partial g(f(\mathbf{X}))}{\partial \mathbf{X}}=\frac{\mathrm{d}g(y)}{\mathrm{d}y}\frac{\partial f(\mathbf{X})}{\partial \mathbf{X}}$$

以下设 $\mathbf{a}、\mathbf{A}$ 为与变量 $\mathbf{x}$ 无关的向量或者矩阵。则
$$\frac{\partial {\mathbf{a}}^{\top }\mathbf{x}}{\partial \mathbf{x}}=\frac{\partial {\mathbf{x}}^{\top }\mathbf{a}}{\partial \mathbf{x}}=\mathbf{a}$$
$$\frac{\partial {\mathbf{x}}^{\top }\mathbf{A}\mathbf{y}}{\partial \mathbf{x}}=\mathbf{A}\mathbf{y},\frac{\partial {\mathbf{y}}^{\top }\mathbf{A}\mathbf{x}}{\partial \mathbf{x}}={\mathbf{A}}^{\top }\mathbf{y}$$
$$\frac{\partial {\mathbf{x}}^{\top }\mathbf{A}}{\partial \mathbf{x}}=\mathbf{A},\frac{\partial {\mathbf{x}}^{\top }\mathbf{A}\mathbf{x}}{\partial \mathbf{x}}=\left(\mathbf{A}+{\mathbf{A}}^{\top }\right)\mathbf{x}$$
设 $f=\frac{1}{2}{\mathbf{x}}^{\top }\mathbf{Q}\mathbf{x}$, 其中 $\mathbf{Q}$ 为对称矩阵。则 $$\nabla f=\mathbf{Q}\mathbf{x},{\nabla }^{2}f=\mathbf{Q}$$

---

6.2.2 高斯牛顿法

它的思想是将$f(x)$进行一阶的泰勒展开。注意这里不是目标函数$F(x)$，而是$f(x)$，否则就变成牛顿法了。
$$f(\mathbf{x}+\Delta \mathbf{x})\approx f(\mathbf{x})+\mathbf{J}(\mathbf{x})\Delta \mathbf{x}\text{\hspace{1em}(6.30)}$$
这里 $\mathbf{J}(\mathbf{x}{)}^{\mathrm{T}}$ 为 $f(\mathbf{x})$ 关于 $\mathbf{x}$ 的导数, 为 $n\times 1$ 的列向量。根据前面的框架, 当前的目标是寻找增 量 $\Delta x$ , 使得 $\Vert f(x+\Delta x){\Vert }^2$ 达到最小。为了求 $\Delta x$ , 我们需要解一个线性的最小二乘问题:
$$\Delta x^{\ast }=\arg \underset{\Delta x}{\min }\frac{1}{2}{\Vert f(x)+J(x)\Delta x\Vert }^2\text{\hspace{1em}(6.31)}$$
注：必须说明的是，《视觉SLAM十四讲》里的式(6.30)泰勒展开有误，展开后的雅克比矩阵是不需要转置的
$$\begin{array}{rl}\frac{1}{2}{\Vert f(\mathbf{x})+\mathbf{J}(\mathbf{x})\Delta \mathbf{x}\Vert }^2& =\frac{1}{2}{\left(f(\mathbf{x})+\mathbf{J}(\mathbf{x})\Delta \mathbf{x}\right)}^{\mathrm{T}}\left(f(\mathbf{x})+\mathbf{J}(\mathbf{x})\Delta \mathbf{x}\right)\\ & =\frac{1}{2}\left(f(\mathbf{x}{)}^{\mathrm{T}}+\Delta {\mathbf{x}}^{\mathrm{T}}\mathbf{J}(\mathbf{x}{)}^{\mathrm{T}}\right)\left(f(\mathbf{x})+\mathbf{J}(\mathbf{x})\Delta \mathbf{x}\right)\\ & =\frac{1}{2}\left(f(\mathbf{x}{)}^{\mathrm{T}}f(\mathbf{x})+f(\mathbf{x}{)}^{\mathrm{T}}\mathbf{J}(\mathbf{x})\Delta \mathbf{x}+\Delta {\mathbf{x}}^{\mathrm{T}}\mathbf{J}(\mathbf{x}{)}^{\mathrm{T}}f(\mathbf{x})+\Delta {\mathbf{x}}^{\mathrm{T}}\mathbf{J}(\mathbf{x}{)}^{\mathrm{T}}\mathbf{J}(\mathbf{x})\Delta \mathbf{x}\right)\end{array}\text{\hspace{1em}(6.32)}$$
求上式关于 $\Delta x$ 的导数, 并令其为零:
$$\mathbf{J}(\mathbf{x}{)}^{\mathrm{T}}f(\mathbf{x})+\mathbf{J}(\mathbf{x}{)}^{\mathrm{T}}\mathbf{J}(\mathbf{x})\Delta \mathbf{x}=0\text{\hspace{1em}(6.33)}$$
可以得到如下方程组:
$$\underset{\mathbf{H}(\mathbf{x})}{{\mathbf{J}}^{\mathrm{T}}(\mathbf{x})\mathbf{J}}(\mathbf{x})\Delta \mathbf{x}=\underset{\mathbf{g}(\mathbf{x})}{-{\mathbf{J}}^{\mathrm{T}}(\mathbf{x})f(\mathbf{x})}.\text{\hspace{1em}(6.34)}$$
这个方程是关于变量 $\Delta x$ 的线性方程组, 我们称它为增量方程, 也可以称为高斯牛顿方程 ( Gauss-Newton equation) 或者正规方程 (Normal equation )。我们把左边的系数定义为 $\mathbf{H}$ , 右边 定义为 $g$ , 那么上式变为
$$\mathbf{H}\Delta \mathbf{x}=\mathbf{g}\text{\hspace{1em}(6.35)}$$

这里把左侧记作$\mathbf{H}$是有意义的。对比牛顿法可见，高斯牛顿法用$\mathbf{J}(\mathbf{x}){\mathbf{J}}^{\mathrm{T}}$作为牛顿法中二阶海森矩阵的近似，从而省略了计算$\mathbf{H}$的过程。

高斯牛顿法的缺陷

为了求解增量方程，我们需要求解${\mathbf{H}}^{-1}$，但实际数据中计算得到的$\mathbf{J}(\mathbf{x}){\mathbf{J}}^{\mathrm{T}}$只有半正定性。也就是说，求${\mathbf{H}}^{-1}$时可能无解，导致算法不收敛。直观地说，原函数在这个点的局部近似不像一个二次函数。因此，每次求逆都需要判断判断是否求逆成功。

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

高斯牛顿法中采用的近似二阶泰勒展开只能在展开点附近有较好的近似效果，所以我们应该给$\Delta \mathbf{x}$添加一个范围，称为信赖区域。这个范围定义了在什么情况下二阶近似是有效的。

那么，如何确定这个信赖区域的范围呢？一个比较好的方法是根据我们的近似模型跟实际函数之间的差异来确定：如果差异小，说明近似效果好，我们扩大近似的范围；反之，如果差异搭，就缩小近似的范围。我们定义一个指标$\rho$来刻画近似的好坏程度：
$$\rho =\frac{f(\mathbf{x}+\Delta \mathbf{x})-f(\mathbf{x})}{\mathbf{J}(\mathbf{x}{)}^{\mathrm{T}}\Delta \mathbf{x}}\text{\hspace{1em}(6.34)}$$
$\rho$的分子式实际函数下降的值，分母式近似模型下降的值。

1. 如果$\rho$接近于1，则近似是好的。
2. 如果$\rho$太小，说明实际减小的值远少于近似减小的值，则认为近似比较差。需要缩小近似范围。
3. 如果$\rho$比较大，则说明实际下降的比预计的更大，我们可以放大近似范围。

算法流程：

1. 给定初始值${\mathbf{x}}_0$，以及初始优化半径$\mu$。
2. 对于第$k$次迭代，在高斯牛顿法的基础上加上信赖区域，求解：
   $$\underset{\Delta {\mathbf{x}}_k}{\min }\frac{1}{2}{\Vert f\left({\mathbf{x}}_k\right)+\mathbf{J}{\left({\mathbf{x}}_k\right)}^{\mathrm{T}}\Delta {\mathbf{x}}_k\Vert }^2,\text{ s.t. }{\Vert \mathbf{D}\Delta {\mathbf{x}}_k\Vert }^2⩽\mu \text{\hspace{1em}(6.35)}$$
   其中，$\mu$是信赖区域的半径，$\mathbf{D}$为系数矩阵
3. 按照式(6.34)计算$\rho$。
4. 若$\rho >\frac{3}{4}$，则设置$\mu =2\mu$
5. 若$\rho <\frac{1}{4}$，则设置$\mu =0.5\mu$
6. 如果$\rho$大于某阈值，则认为近似可行。令${\mathbf{x}}_{k+1}={\mathbf{x}}_k+\Delta {\mathbf{x}}_k$
7. 判断算法是否收敛。如不收敛则返回第2步，否则结束。

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

这个子问题是带不等式约束的优化问题，我们用拉格朗日乘子把约束项放到目标函数中，构成拉格朗日函数：
$$\mathcal{L}\left(\Delta {\mathbf{x}}_k,\lambda \right)=\frac{1}{2}{\Vert f\left({\mathbf{x}}_k\right)+\mathbf{J}{\left({\mathbf{x}}_k\right)}^{\mathrm{T}}\Delta {\mathbf{x}}_k\Vert }^2+\frac{\lambda }{2}\left({\Vert \mathbf{D}\Delta {\mathbf{x}}_k\Vert }^2-\mu \right)\text{\hspace{1em}(6.36)}$$
这里 $\lambda$ 为拉格朗日乘子。类似于高斯牛顿法中的做法, 令该拉格朗日函数关于 $\Delta x$ 的导数为零, 它的核心仍是计算增量的线性方程:
$$\left(\mathbf{H}+\lambda {\mathbf{D}}^{\mathrm{T}}\mathbf{D}\right)\Delta {\mathbf{x}}_k=\mathbf{g}\text{\hspace{1em}(6.37)}$$
可以看到, 相比于高斯牛顿法, 增量方程多了一项 $\lambda {\mathbf{D}}^T\mathbf{D}$ 。如果考虑它的简化形式（列文伯格提出）, 即 $\mathbf{D}=\mathbf{I}$ , 那么相当于求解 :
$$(\mathbf{H}+\lambda \mathbf{I})\Delta {\mathbf{x}}_k=\mathbf{g}\text{\hspace{1em}(6.37-1)}$$

• 当参数$\lambda$比较小时，$\mathbf{H}$占主要地位，这说明二次近似模型在该范围内是比较好的，列文伯格-马夸尔特方法更接近于高斯牛顿法
• 当参数$\lambda$比较大时，$\lambda \mathbf{I}$占据主要地位，列文伯格-马夸尔特方法更接近于最速下降法，这说明附近的二次近似不够好。

进一步地，马夸尔特提出了通过对角线元素增加比例系数来提供更快的收敛速度：
$$\left(\mathbf{J}(\mathbf{x}{)}^{\mathrm{T}}\mathbf{J}(\mathbf{x})+\lambda \text{diag}\left(\mathbf{J}(\mathbf{x})\mathbf{J}(\mathbf{x}{)}^{\mathrm{T}}\right)\right)\Delta {\mathbf{x}}_k=-\mathbf{J}(\mathbf{x})f(\mathbf{x})\text{\hspace{1em}(6.37-2)}$$

• 如果梯度值很小（目标函数几乎是平面的区域），对角线元素的倒数将会变得很大，那么这个修改就会导致在梯度方向上产生较大的步长。
• 反之，在目标函数的梯度方向上，算法将变得更加保守，并采取更小的步长。

式(6.37-1)和式(6.37-2)对正规方程$\mathbf{H}\Delta \mathbf{x}=\mathbf{g}$的修改都可以从贝叶斯的角度理解为给系统增加了一个零均值的先验信息。

列文伯格-马夸尔特算法伪代码

高斯牛顿法和列文伯格-马夸尔特算法之间最关键的区别在于，后者会拒绝可能导致更大的残差平方和的更新，而前者不会。一个更新被拒绝，说明非线性函数在局部附近表现不好，所以需要更小的更新步长。缩小更新步长可以通过启发式地增大$\lambda$的值来实现，例如，可以将现有的$\lambda$值乘以因子10，并且求解修改后的正规方程。另一方面，如果一个步长导致了残差平方和的减小，这个步长就会被接受，并且状态估计值也会相应地被更新。在这种情况下，减小$\lambda$（通过将$\lambda$除以10），算法在一个新的线性化点处继续迭代，直到收敛为止。

6.2.4 Dogleg最小化法（占坑）

待续。。。

6.3 实践：曲线拟合问题

考虑一条满足以下方程的曲线:
$$y=\exp \left(a{x}^2+bx+c\right)+w,$$
其中, $a,b,c$ 为曲线的参数, $w$ 为高斯噪声, 满足 $w\sim \left(0,{\sigma }^2\right)$ 。现在, 假设我们有 $N$ 个关于 $x,y$ 的观测数据点, 想根据这些数据点求出曲线的参数。那么, 可以求解下面的最小二乘问题以估计曲线参数:
$$\underset{a,b,c}{\min }\frac{1}{2}\sum _{i=1}^N{\Vert y_i-\exp \left(a{x}_i^2+b{x}_i+c\right)\Vert }^2$$
请注意, 在这个问题中, 待估计的变量是 $a,b,c$ , 而不是 $x$ 。我们的程序里先根据模型生成 $x,y$ 的真值, 然后在真值中添加高斯分布的噪声。随后, 使用高斯牛顿法从带噪声的数据拟合参数模型。定义误差为
$$e_i=y_i-\exp \left(a{x}_i^2+b{x}_i+c\right),$$
那么, 可以求出每个误差项对于状态变量的导数:
$$\begin{array}{l}\frac{\partial e_i}{\partial a}=-x_i^2\exp \left(a{x}_i^2+b{x}_i+c\right)\\ \frac{\partial e_i}{\partial b}=-x_i\exp \left(a{x}_i^2+b{x}_i+c\right)\\ \frac{\partial e_i}{\partial c}=-\exp \left(a{x}_i^2+b{x}_i+c\right)\end{array}$$
于是 ${\mathbf{J}}_i={\left[\frac{\partial e_i}{\partial a},\frac{\partial e_i}{\partial b},\frac{\partial e_i}{\partial c}\right]}^{\mathrm{T}}$ , 高斯牛顿法的增量方程为
$$\left(\sum _{i=1}^{100}{J}_i{\left({\sigma }^2\right)}^{-1}{J}_i{}^{\top }\right)\Delta x_k=\sum _{i=1}^{100}-J_i{\left({\sigma }^2\right)}^{-1}{e}_{i,}$$
为了让求解过程更加清晰，我们选择把所有的${\mathbf{J}}_i$排成一列，将这个方程写成矩阵形式。不过这样会消耗较多的内存。

6.3.2 手写高斯牛顿法

#include 
#include 
#include 
#include 
#include 

using namespace Eigen;

int main(int argc, char** argv) {
    // 配置真实值
    double ar = 1.0, br = 2.0, cr = 1.0;
    // 配置噪声值
    double w_sigma = 1.0;
    double inv_w_sigma = 1.0/w_sigma;
    // 生成数据点
    int N = 100;
    // OpenCV随机数生成器
    cv::RNG rng; 

    // 雅克比矩阵
    Eigen::Matrix<double, 100, 3> J;
    J.setZero();
    // 正规矩阵H = J^T*J
    Eigen::Matrix<double, 3, 3> H;
    H.setZero();
    // J_n
    Eigen::Matrix<double, 1, 3> Jn;
    Jn.setZero();
    // f(x)
    Eigen::Matrix<double, 100, 1> f;
    f.setZero();
    // g(x)
    Eigen::Matrix<double, 3, 1> g;
    g.setZero();
    // /Delta x
    Eigen::Matrix<double, 3, 1> Delta_x;
    Delta_x.setZero();

    std::vector<double> x_data, y_data;
    for(size_t i=0; i<N; i++)
    {
        double x = i/100.0;
        x_data.push_back(x);
        y_data.push_back(std::exp(ar*x*x+br*x+cr)+rng.gaussian(w_sigma*w_sigma));
    }

    // 设置初始值
    double ae = 2.0, be = -1.0, ce = 5.0;

    // 开始最速下降法迭代
        // 设置迭代次数
        // 设置结束条件
    size_t numIter = 100;
    double cost = 0.0;
    double last_cost = 0.0;
    size_t count = 0;
    std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
    for(size_t i=0; i<numIter; i++)
    {
        for(size_t j=0; j<N; j++)
        {
            Jn(0, 0) = x_data[j]*x_data[j]*std::exp(ae*x_data[j]*x_data[j]+be*x_data[j]+ce);
            Jn(0, 1) = x_data[j]*std::exp(ae*x_data[j]*x_data[j]+be*x_data[j]+ce);
            Jn(0, 2) = 1*std::exp(ae*x_data[j]*x_data[j]+be*x_data[j]+ce);
            J.block(j, 0, 1, 3) = inv_w_sigma*Jn; // 记得配置上噪声权值
            f(j, 0) = inv_w_sigma*std::exp(ae*x_data[j]*x_data[j]+be*x_data[j]+ce) - y_data[j]; // 记得配置上噪声权值
        }
        cost = f.norm();
        H = J.transpose()*J;
        g = -J.transpose()*f;
        // 直接求逆
        // Delta_x = H.inverse()*g;

        // 使用cholesky分解来解方程
        // Delta_x = H.ldlt().solve(g);

        // 使用QR分解来解方程，速度比较快
        Delta_x = H.colPivHouseholderQr().solve(g);
        

        if(isnan(Delta_x[0]))
        {
            std::cout<<"result is nan!"<<std::endl;
            break;
        }

        if(cost>=last_cost && i>0)
        {
            std::cout<<"cost: "<<cost<<" >= last cost: "<<last_cost<<", break."<<std::endl;
            break;
        }

        last_cost = cost;

        ae = ae + Delta_x(0, 0);
        be = be + Delta_x(1, 0);
        ce = ce + Delta_x(2, 0);

        count = i;
    }

    std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
    std::chrono::duration<double> time_used = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
    std::cout << "solve time cost = " << time_used.count() << " seconds. " << std::endl;
    std::cout<<"estimated abc = "<<ae<<", "<<be<<", "<<ce<<std::endl;
    std::cout<<"number of iterations = "<<count<<std::endl;
    return 0;
    
}

6.3.1 手写列文伯格-马夸尔特方法

#include 
#include 
#include 
#include 
#include 

using namespace Eigen;

int main(int argc, char** argv) {
    // 配置真实值
    double ar = 1.0, br = 2.0, cr = 1.0;
    // 配置噪声值
    double w_sigma = 1.0;
    double inv_w_sigma = 1.0/w_sigma;
    // 生成数据点
    int N = 100;
    // OpenCV随机数生成器
    cv::RNG rng; 

    double lambda = 1e-4;

    // 雅克比矩阵
    Eigen::Matrix<double, 100, 3> J;
    J.setZero();
    // 正规矩阵H = J^T*J
    Eigen::Matrix<double, 3, 3> H;
    H.setZero();
    // J_n
    Eigen::Matrix<double, 1, 3> Jn;
    Jn.setZero();
    // f(x)
    Eigen::Matrix<double, 100, 1> f;
    f.setZero();
    // g(x)
    Eigen::Matrix<double, 3, 1> g;
    g.setZero();
    // /Delta x
    Eigen::Matrix<double, 3, 1> Delta_x;
    Delta_x.setZero();

    Eigen::Matrix3d DeltaMatrix;
    DeltaMatrix.setZero();

    std::vector<double> x_data, y_data;
    for(size_t i=0; i<N; i++)
    {
        double x = i/100.0;
        x_data.push_back(x);
        y_data.push_back(std::exp(ar*x*x+br*x+cr)+rng.gaussian(w_sigma*w_sigma));
    }

    // 设置初始值
    double ae = 2.0, be = -1.0, ce = 5.0;

    // 开始最速下降法迭代
        // 设置迭代次数
        // 设置结束条件
    size_t numIter = 100;
    double cost = 0.0;
    double last_cost = 0.0;
    size_t count = 0;
    std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
    for(size_t i=0; i<numIter; i++)
    {
        for(size_t j=0; j<N; j++)
        {
            Jn(0, 0) = x_data[j]*x_data[j]*std::exp(ae*x_data[j]*x_data[j]+be*x_data[j]+ce);
            Jn(0, 1) = x_data[j]*std::exp(ae*x_data[j]*x_data[j]+be*x_data[j]+ce);
            Jn(0, 2) = 1*std::exp(ae*x_data[j]*x_data[j]+be*x_data[j]+ce);
            J.block(j, 0, 1, 3) = inv_w_sigma*Jn; // 记得配置上噪声权值
            f(j, 0) = inv_w_sigma*std::exp(ae*x_data[j]*x_data[j]+be*x_data[j]+ce) - y_data[j]; // 记得配置上噪声权值
        }
        cost = f.norm();
        H = J.transpose()*J;
        Eigen::Matrix3d diagMatrix = Eigen::Matrix3d(H.diagonal().asDiagonal());
        DeltaMatrix = H + lambda*diagMatrix;
        g = -J.transpose()*f;

        // 直接求逆
        // Delta_x = DeltaMatrix.inverse()*g;

        // 使用cholesky分解来解方程
        // Delta_x = DeltaMatrix.ldlt().solve(g);

        // 使用QR分解来解方程，速度比较快
        Delta_x = DeltaMatrix.colPivHouseholderQr().solve(g);
        

        if(isnan(Delta_x[0]))
        {
            std::cout<<"result is nan!"<<std::endl;
            break;
        }

        if(Delta_x.norm()<0.001)
        {
            std::cout<<"result is converge!"<<std::endl;
            break;
        }

        // 接受更新
        if((cost<last_cost && i>0) || i==0)
        {
            ae = ae + Delta_x(0, 0);
            be = be + Delta_x(1, 0);
            ce = ce + Delta_x(2, 0);

            if(i>0) lambda*=0.1;
        }
        // 发散了，拒绝更新
        else
        {
            lambda*=10.0;
        }
        
        last_cost = cost;

        count = i;
    }

    std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
    std::chrono::duration<double> time_used = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
    std::cout << "solve time cost = " << time_used.count() << " seconds. " << std::endl;
    std::cout<<"estimated abc = "<<ae<<", "<<be<<", "<<ce<<std::endl;
    std::cout<<"number of iterations = "<<count<<std::endl;
    return 0;
    
}

6.3.3 使用Ceres进行曲线拟合（占坑）

学习Ceres-solver最全的文档：Ceres Solver

代码以后再补…

6.3.4 使用g2o进行曲线拟合（占坑）

在图优化库里，g2o和GTSAM的功能类似，而我自己更喜欢用GTSAM，因为它集成了ISAM2。因此这里暂时搁置g2o。

6.3.5 使用GTSAM进行曲线拟合（占坑）

学习GTSAM最全的文档：GTSAM

代码以后再补…