以SLAMer角度看待GN和LM算法_为什么slam用lm

引言部分（废话）

SLAM作为机器人和自动驾驶领域的核心问题之一，其目标是在未知环境中实现同时的定位和地图构建。其中在求解多观测约束下的位姿估计问题时，GN和LM算法在求解非线性最小二乘问题方面扮演了重要的角色。为嘛呢？因为这俩思想非常简单实用。博主个人认为，这两种方法的一个共同的优点是快，简单，准确度可以接受。接下来，让我们从一个SLAMer的角度看待GN和LM算法。注意，本篇不涉及代码编程和复杂的理论说明，仅有公式推导，旨在用白话来讨论GN和LM算法。如有逻辑上的不严谨，欢迎各位批评指证。

---

1. 一个非线性优化问题的建模

  一个SLAM问题通常可以被描述为一个最大后验概率问题，也叫MAP（Maximum ）问题， 即我们想根据历史时刻已观测到的数据$z$和状态$x$，以及控制量$u$，得到最有可能的下一时刻的状态$x_{t+1}$（因为SLAM的核心本质是Localization嘛），用公式来描述就是
$$x_{t+1}=\underset{x}{\mathrm{argmax}}P(x_{t+1}|z_{0:t},x_{0:t},u_{0:t})$$
  这其实是一个很抽象的描述公式，因为我们能通过公式看到SLAM问题的目的是什么，但是我们无法找到一个具体的代入方法和求解方法。并且，求解MAP问题本身是一个比较复杂的问题，求解方法也有很多种，比如 粒子滤波，卡尔曼滤波等等。而本章所说的非线性优化，也是求解MAP问题的一个有效的途径。
  我们以一个纯激光SLAM为例，激光雷达带来的是一系列的点云，我们可以把每一个点都看作是一次对环境的观测，对一个SLAM（或者是Localization）问题，最重要的是要推算出雷达的当前位置${\mathbf{x}}_t^L$，而且我们还知道在一个3维空间下的刚性变换$\mathbf{T}$可以用旋转矩阵$\mathbf{R}$和平移向量$\mathbf{t}$来表示，那么我们可以将雷达的当前位置表示为

$${\mathbf{x}}_t^L=\prod _{i=0}^{t-1}{△\mathbf{T}}_i\cdot {\mathbf{x}}_0^L$$

用大白话讲，雷达当前的位置相当于对雷达初始位置${\mathbf{x}}_0^L$逐步进行变换${△\mathbf{T}}_i$即可，那么我们的问题就转换为如何求取${△\mathbf{T}}_i$。
  现在我们假设，经过一个刚性变换$\mathbf{T}$的前后两帧雷达点云分别为 P A = { p i A } P_{A}=\{ p^{A}_{i} \} PA​={piA​}和 P B = { p j } \mathcal{P}_{B}=\{ \mathbf{p}_{j} \} PB​={pj​}，并我们在不考虑噪声和异常点（outlier）的前提下，这两坨点云理应满足
P B = T P A ∑ ∣ ∣ p j B − T p i A ∣ ∣ 2 = 0

$$\begin{matrix} \mathcal{P}_{B}= \mathbf{T}\mathcal{P}_{A} \\\\ \sum ||\mathbf{p}^{B}_{j} - \mathbf{T} \mathbf{p}^{A}_{i}||_{2} = 0\end{matrix}$$ PB​=TPA​∑∣∣pjB​−TpiA​∣∣2​=0​
这两个式子意思就是，变换前的雷达帧历经一次刚性变换$\mathbf{T}$后，应该与变换后的点云完全重合（在点的对应关系已知的情况下）。But！这只是一个非常理想的情况，现实情境下，是不可以忽略噪声和异常点的！

  OK，那么我们现在将这些因素考虑进去。假设我们给每个点加一个高斯白噪声，即${\mathbf{p}}_i\sim N({\mathbf{p}}_i,{\mathbf{\Sigma }}_i)$， 这里由于一个点可做是一个3维向量，因此每个点服从一个多维高斯分布。也就是每个点的位置分布情况是涉及到一个概率的，我们知道高斯分布有个优雅的性质，多个独立的高斯分布的线性组合依旧是高斯分布，那么我们可以推出

$$({\mathbf{p}}_j^B-\mathbf{T}{\mathbf{p}}_i^A)\sim N({\mathbf{d}}_{ij},{\mathbf{\Sigma }}_j+{\mathbf{T}}^T{\mathbf{\Sigma }}_i\mathbf{T})$$
哎！？这个好，这个公式看起来很优雅，这时我们就在想，如果每个点之间都是独立的，这个$\mathbf{T}$只要能让每个点对的差值${\mathbf{d}}_{ij}$为0的联合概率最大不就完了么！！吆西，那么说做就做，我们得到下面这个式子
T = a r g m a x T ∏ P ( p j B − T p i A ) = l o g a r g m i n T ∑ d i j T ( Σ j + T T Σ i T ) − 1 d i j = a r g m i n T ∑ d i j T ( Σ i j ) − 1 d i j

$$\begin{align} \mathbf{T} & = \underset{\mathbf{T} }{argmax} \prod P(\mathbf{p}^{B}_{j} - \mathbf{T} \mathbf{p}^{A}_{i})\\ & \overset{log}{=} \underset{\mathbf{T} }{argmin} \sum \mathbf{d}_{ij}^{T}(\mathbf{\Sigma}_{j} + \mathbf{T}^{T} \mathbf{\Sigma}_{i} \mathbf{T})^{-1} \mathbf{d}_{ij}\\ & = {\color{Red} \underset{\mathbf{T} }{argmin} \sum \mathbf{d}_{ij}^{T}(\mathbf{\Sigma}_{ij})^{-1} \mathbf{d}_{ij}} \end{align}$$ T​=Targmax​∏P(pjB​−TpiA​)=logTargmin​∑dijT​(Σj​+TTΣi​T)−1dij​=Targmin​∑dijT​(Σij​)−1dij​​​
解释一下，公式（1）表示的是联合概率最大化，但是求积操作不好算，况且高斯分布里含有指数$exp(\cdot )$，所以在这里两边取对数操作来去掉指数和常数项，化积为和。最终化简出来的公式（3）正式描述了一个非线性最小二乘问题，也是ICP配准问题的核心公式。

  现在我们换一个思路来理解上面公式（3）并且将里面的符号转换成一个易于理解的形式。众所周知，最小二乘问题的本质思想是：让真实值与理论值的差距之和（即残差）最小。从这个思路出发，我们可以发现，公式（3）中的${\mathbf{d}}_{ij}^T{\mathbf{d}}_{ij}$其实就是最小二乘中的残差项，而$({\mathbf{\Sigma }}_{ij}{)}^{-1}$只不过是给每一个残差项成了一个权重系数，因此博主习惯把$({\mathbf{\Sigma }}_{ij}{)}^{-1}$称为权重矩阵（当然这个矩阵的名号很多，比如说信息矩阵，马氏距离…）。这个如果点对的协方差越小，代表对位置的信任程度，就越高，自然权重就越大。OK，这样一个针对非线性优化的最小二乘问题就描述完了！ 下面呢，为了便于理解，我们还是用${\mathbf{e}}_{ij}$表示残差，用${\mathbf{\Omega }}_{ij}$表示权重矩阵（信息矩阵），即
$$\mathbf{T}=\underset{\mathbf{T}}{\mathrm{argmin}}\sum {\mathbf{e}}_{ij}^T{\mathbf{\Omega }}_{ij}{\mathbf{e}}_{ij}(LSQ)$$

2. 高斯牛顿优化算法（Gauss-Newton）

  我们现在有要解决的目标优化问题，那么该如何求解呢，梯度下降法教会了我们通过迭代求解最小值时，要往梯度（导数）下降最大的方向走，但是如何求梯度呢？这个时候高斯牛顿法对此有了新的方案。
  话不多说，我们开始进入正题（原谅博主废话过多），第1章中每一个残差项可以看作是一个关于${\mathbf{p}}^A$的函数（因为我们是想让通过调整${\mathbf{P}}_A$来对齐${\mathbf{P}}_B$），以下简称$\mathbf{e}(\mathbf{p})$。现在我们对$\mathbf{e}(\mathbf{p})$在$\mathbf{p}$附近进行一阶泰勒展开。
$$\mathbf{e}(\mathbf{p}+\Delta \mathbf{p})=\mathbf{e}(\mathbf{p})+\mathbf{J}(\mathbf{p})\Delta \mathbf{p}(Taylor)$$
这里的$\mathbf{J}(\mathbf{p})=\frac{d\mathbf{e}(\mathbf{p})}{d\mathbf{p}}$ ，也就是江湖上令人闻风丧胆的雅可比矩阵（Jacobian Matrix），为啥闻风丧胆？且看后续推导，现在我们只需要假设已知$\mathbf{J}(\mathbf{p})$就好啦。

这里需要说明两点：

1. 泰勒的意义是什么，在G-N优化中，我们并不是直接求两坨点云之间的变换$\mathbf{T}$，而是通过迭代求解，每次迭代会寻找一个的增量$\Delta \mathbf{T}$，使得$\sum {\mathbf{e}}_k^T(\Delta \mathbf{T}{\mathbf{p}}_k){\mathbf{\Omega }}_k{\mathbf{e}}_k(\Delta \mathbf{T}{\mathbf{p}}_k)$尽可能的小
2. 符号的说明，有人就要怼我了，怎么还一会用$\Delta \mathbf{Tp}$，一会用$\mathbf{p}+\Delta \mathbf{p}$。其实这里两个表示方法是同一个意思，都表示为对一个点做一次刚性变换，只不过在推导数学公式时，我们可能更习惯使用求和而已。

  这里我们旨在求解每次迭代过程中的最优$\Delta \mathbf{p}$即可， 我们将上面泰勒展开式子代入到公式（LSQ）中，并进行推导
△ p = a r g m i n △ p ∑ e k T ( p + △ p ) Ω k e k ( p + △ p ) = a r g m i n △ p ∑ [ e k ( p ) + J k ( p ) Δ p ] T Ω k [ e k ( p ) + J k ( p ) Δ p ] = a r g m i n △ p ∑ ( e k T Ω k e k + e k T Ω k J k △ p + △ p T J k T Ω k e k + △ p T J k T Ω k J k △ p ) = a r g m i n △ p ∑ ( e k T Ω k e k ⏟ 常数项 + 2 ⋅ △ p T J k T Ω k e k + △ p T J k T Ω k J k △ p ) = a r g m i n △ p ∑ ( 2 ⋅ △ p T J k T Ω k e k + △ p T J k T Ω k J k △ p ) = a r g m i n △ p [ 2 △ p T ⋅ ∑ J k T Ω k e k + △ p T ⋅ ∑ J k T Ω k J k ⋅ △ p ) ]

$$\begin{align} \bigtriangleup \mathbf{p} & = \underset{\mathbf{\bigtriangleup p} }{argmin} \sum \mathbf{e}_{k}^{T}(\mathbf{p} +\mathbf{\bigtriangleup p} )\mathbf{\Omega }_{k} \mathbf{e}_{k}(\mathbf{p} +\mathbf{\bigtriangleup p}) \\ & = \underset{\mathbf{\bigtriangleup p} }{argmin} \sum [\mathbf{e}_{k}(\mathbf{p}) + \mathbf{J}_{k} (\mathbf{p})\Delta\mathbf{p}]^{T} \mathbf{\Omega }_{k} [\mathbf{e}_{k}(\mathbf{p}) + \mathbf{J}_{k} (\mathbf{p})\Delta\mathbf{p}] \\ & = \underset{\mathbf{\bigtriangleup p} }{argmin} \sum (\mathbf{e}_{k}^{T}\Omega_{k}\mathbf{e}_{k} +\mathbf{e}_{k}^{T}\Omega_{k}\mathbf{J}_{k}\mathbf{\bigtriangleup p} + \mathbf{\bigtriangleup p}^{T}\mathbf{J}_{k}^{T}\Omega_{k}\mathbf{e}_{k}+\mathbf{\bigtriangleup p}^{T}\mathbf{J}_{k}^{T}\Omega_{k}\mathbf{J}_{k}\mathbf{\bigtriangleup p}) \\ & = \underset{\mathbf{\bigtriangleup p} }{argmin} \sum (\underbrace{\mathbf{e}_{k}^{T}\Omega_{k}\mathbf{e}_{k}}_{常数项} +2\cdot \mathbf{\bigtriangleup p}^{T}\mathbf{J}_{k}^{T}\Omega_{k}\mathbf{e}_{k}+\mathbf{\bigtriangleup p}^{T}\mathbf{J}_{k}^{T}\Omega_{k}\mathbf{J}_{k}\mathbf{\bigtriangleup p}) \\ & = \underset{\mathbf{\bigtriangleup p} }{argmin} \sum (2\cdot \mathbf{\bigtriangleup p}^{T}\mathbf{J}_{k}^{T}\Omega_{k}\mathbf{e}_{k}+\mathbf{\bigtriangleup p}^{T}\mathbf{J}_{k}^{T}\Omega_{k}\mathbf{J}_{k}\mathbf{\bigtriangleup p}) \\ & = \underset{\mathbf{\bigtriangleup p} }{argmin} [2 \mathbf{\bigtriangleup p}^{T}\cdot \sum \mathbf{J}_{k}^{T}\Omega_{k}\mathbf{e}_{k}+\mathbf{\bigtriangleup p}^{T}\cdot \sum \mathbf{J}_{k}^{T}\Omega_{k}\mathbf{J}_{k} \cdot \mathbf{\bigtriangleup p})] \\ \end{align}$$ △p​=△pargmin​∑ekT​(p+△p)Ωk​ek​(p+△p)=△pargmin​∑[ek​(p)+Jk​(p)Δp]TΩk​[ek​(p)+Jk​(p)Δp]=△pargmin​∑(ekT​Ωk​ek​+ekT​Ωk​Jk​△p+△pTJkT​Ωk​ek​+△pTJkT​Ωk​Jk​△p)=△pargmin​∑(常数项 ekT​Ωk​ek​​​+2⋅△pTJkT​Ωk​ek​+△pTJkT​Ωk​Jk​△p)=△pargmin​∑(2⋅△pTJkT​Ωk​ek​+△pTJkT​Ωk​Jk​△p)=△pargmin​[2△pT⋅∑JkT​Ωk​ek​+△pT⋅∑JkT​Ωk​Jk​⋅△p)]​​
其中，博主为了简化表示，将${\mathbf{e}}_k(\mathbf{p})$简化为${\mathbf{e}}_k$，得到的公式（9）非常重要，它表明，该目标优化问题是一个关于$\Delta \mathbf{p}$的二次型问题！这个发现很重要，决定了优化求解的稳定性。
  接下来，这里我们计算公式（9）中的目标函数关于$\Delta \mathbf{p}$的导数，可以得到
$$\begin{array}{cc}& [2{△\mathbf{p}}^T\cdot \sum {\mathbf{J}}_k^T{\Omega }_k{\mathbf{e}}_k+{△\mathbf{p}}^T\cdot \sum {\mathbf{J}}_k^T{\Omega }_k{\mathbf{J}}_k\cdot △\mathbf{p}){]}^{\prime }\\ & \\ & =2\sum {\mathbf{J}}_k^T{\Omega }_k{\mathbf{e}}_k+2\sum {\mathbf{J}}_k^T{\Omega }_k{\mathbf{J}}_k\cdot △\mathbf{p}\end{array}$$
令其导数$2\sum {\mathbf{J}}_k^T{\Omega }_k{\mathbf{e}}_k+2\sum {\mathbf{J}}_k^T{\Omega }_k{\mathbf{J}}_k\cdot △\mathbf{p}=0$，我们可以得到
$$2\sum {\mathbf{J}}_k^T{\Omega }_k{\mathbf{J}}_k\cdot △\mathbf{p}+2\sum {\mathbf{J}}_k^T{\Omega }_k{\mathbf{e}}_k=0$$
可能你还觉得不够面熟，我们找个符号代替一下：
$$\begin{array}{cc}& \mathbf{H}△\mathbf{p}-\mathbf{g}=0（GN中的增量方程）\\ where,& \\ & \mathbf{H}=\sum {\mathbf{J}}_k^T{\Omega }_k{\mathbf{J}}_k,\\ & \\ & \mathbf{g}=-\sum {\mathbf{J}}_k^T{\Omega }_k{\mathbf{e}}_k\end{array}$$
这样就熟悉多了，这不就是在线性代数课上学的线性方程求解问题么。没错是的，我们通过化简最终将优化问题转化为一个求解线性方程的问题，怎么求解，这个就不用多说了。这里的$\mathbf{H}$，通常被称为海森矩阵（Hessian Matrix），而且海森矩阵$\mathbf{H}$要求必须是正定矩阵，上述的化简才是有意义的，为啥嘞？我们能够看到这个线性方程的导数就是$\mathbf{H}$（目标函数的二阶导数），我们知道只有当一阶导数为0，二阶导数＞0时（$\mathbf{H}$为正定矩阵），我们求出来的才是严格意义上的最小值点。当然这只是一次迭代所求解的变换$\Delta \mathbf{T}$，多次迭代收敛后，我们就可以得到完整的刚性变换 $\mathbf{T}=\prod \Delta \mathbf{T}$。
  综上所述，给读者朋友们找了（实则网上扒