SLAM公式引出、推导和理解 3

---

4 优化问题—处理噪声

4.1 状态估计

由于SLAM的数学表达式为$$\{\begin{array}{l}{x}_k=f(x_{k-1},u_k)+w_k\\ z_{k,j}=h(y_j,x_k)+v_{k,j}\end{array}$$假设在 $x_k$ 处对路标 $y_j$ 进行了一次观测，对应到图像上的像素 $z_{k,j}$ ，观测方程可以写成$$s{z}_{k,j}=K(R_k{y}_j+t_k)\text{\hspace{1em}(4.1.1)}$$其中 $K$ 为相机内参矩阵，$s$ 为像素点的距离（$R_k{y}_j+t_k$ 的第三个分量）。
通常假设运动方程和观测方程中的噪声 $w_k$ 和 $v_{k,j}$ 满足零均值的高斯分布：$$w_k\sim \mathcal{N}(0，R_k)v_{k,j}\sim \mathcal{N}(0，Q_{k,j})$$其中 $\mathcal{N}$ 表示告诉分布，$0$ 表示零均值，$R_k$ 和 $Q_{k,j}$ 为协方差矩阵。通过带噪声的数据 $z$ 和 $u$ 推断位姿 $x$ 和地图 $y$ (以及它们的概率分布)，这构成了一个状态估计问题。在比较常见且合理的情况下，假设状态量和噪声量服从高斯分布——这意味着在程序中只需存储它们的均值和协方差矩阵即可。均值可以看作对变量最优值的估计，而协方差矩阵则度量了它的不确定性。因此，问题就变成了 当存在一些运动数据和观测数据时，如何估计状态量 $x$ $y$ 的高斯分布。

处理状态估计问题大致有两种方法，一种是把数据从零时刻整合到当前时刻一齐处理，称为批量（batch）的方法；另一种是已知当前时刻的估计状态，通过下一时刻的数据来更新状态，这种增量/渐进（incremental）的方法称为滤波器。相对来说，增量法仅考虑当前时刻的状态估计即可，而批量方法可以在更大的范围达到最优化，被认为优于增量法，是目前视觉SLAM的主流方法。极端情况下，可以让机器人收集所有时刻的数据，再带回计算中心统一处理，这也是SfM（Structure from Motion）的主流做法。显然，这种极端做法无法做到实时性，不符合SLAM的运动场景。所以在SLAM中，也可以用到折衷的方法。例如，固定一些历史轨迹，仅对当前时刻附近的一些轨迹进行优化，称之为滑动窗口估计法。理解批量方法更容易理解增量的办法。

批量法：
从 $1$ 到 $N$ 的所有时刻，假设有 $M$ 个路标点，定义所有时刻机器人位姿和路标点坐标为 x = { x 1 , ⋯   , x N }        y = { y 1 , ⋯   , y M } (4.1.2) x=\{x_1,\cdots,x_N\}~~~~~~y=\{y_1,\cdots,y_M\}\tag{4.1.2} x={x1​,⋯,xN​}      y={y1​,⋯,yM​}(4.1.2)同样地，用不带下标的 $u$ 表示所有时刻的输入，$z$ 表示所有时刻的观测数据。从概率学的角度来看，对机器人的估计就是已知输入数据 $u$ 和观测数据 $z$ 的条件下，求状态 $x$ 和 $y$ 的条件概率分布：$$P(x,y|z,u)\text{\hspace{1em}(4.1.3)}$$(当控制输入未知，只有图像时，即只考虑观测方程带来的数据时，相当于估计 $P(x,y|z)$ 的条件概率分布，此问题称之为SfM，即如何从众多图像中重建三维空间结构。)

先验、后验是概率，似然估计（函数）是前两者的拟合程度。
根据贝叶斯公式
$$p(x|y)=\frac{p(y|x)p(x)}{p(y)}$$
可得，$P(x)$为先验概率，似然估计是先验和后验的拟合程度，可以看成是一个系数。因此有先验乘以似然估计正比于后验估计。故可有下述公式理解：先得到一个条件概率，将此条件概率视为后验概率（因为后验概率是条件概率。似然估计也可以是条件概率，那怎么不看做似然估计呢？是因为我们通过传感器已知$P(z,u)$，因此就知道$p(x,y)$为先验估计）

由贝叶斯法则得$$P(x,y|z,u)=\frac{P(z,u|x,y)P(x,y)}{P(z,u)}\propto \underset{Likelihood}{\underbrace{P(z,u|x,y)}}\underset{Prior}{\underbrace{P(x,y)}}\text{\hspace{1em}(4.1.4)}$$其中式子(4.1.3)称为后验概率，$P(z,u|x,y)$ 称为似然（Likelihood），$P(x,y)$ 称为先验（Prior）。直接求后验分布是困难的，但求一个状态最优估计，使得在该状态下后验概率最大化是可行的：
$$(x,y{)}_{MAP}^{\ast }=\mathrm{argmax}P(x,y|z,u)=\mathrm{argmax}P(z,u|x,y)P(x,y)\text{\hspace{1em}(4.1.5)}$$
(arg max=argument of max)其中 $P(z,u)$ 与状态 $x$ 和 $y$ 无关，可以省略。求解最大后验估计(Maximize a Posterior）等价于最大化似然和先验的乘积。当然，假如不知道机器人位姿或路标大概的位置关系，就没有了先验。求解最大似然估计（Maximize Likelihood Estimation）$$(x,y{)}_{MLE}^{\ast }=\mathrm{argmax}P(z,u|x,y)\text{\hspace{1em}(4.1.6)}$$似然就是指“在现在的位姿下，可能产生怎样的观测数据”。由于已知观测数据($u$ 和 $y$)，最大似然估计就是指在哪种状态下，最能产生现在观测到的数据。

最小二乘
已知噪声项服从高斯分布 $v_{k,j}\sim \mathcal{N}(0,Q_{k,j})$，对于某一时刻观测方程 $z_{k,j}=h(y_j,x_k)+v_{k,j}$ 来说，有$$P(z_{k,j}|x_k,y_j)=\mathcal{N}(h(y_j,x_k),Q_{k,j})\text{\hspace{1em}(4.1.7)}$$对于最大似然估计，可以用最小化负对数处理此问题。似然估计依然服从高斯分布。由任意高维高斯分布 $x\sim \mathcal{N}(\mu ,\Sigma )$ 的概率密度函数展开式为$$P(x)=\frac{1}{\sqrt{(2\pi {)}^N\det (\Sigma )}}\exp \left(-\frac{1}{2}(x-\mu {)}^T{\Sigma }^{-1}(x-\mu )\right)$$取负对数，则有$$-\ln (P(x))=\frac{1}{2}\ln \left((2\pi {)}^N\det (\Sigma )\right)+\frac{1}{2}(x-\mu {)}^T{\Sigma }^{-1}(x-\mu )\text{\hspace{1em}(4.1.8)}$$因为式子(4.1.8)第一项与 $x$ 无关，可以略去。因此最大似然估计可转变为最小化式子(4.1.8)第二项。因为代入式子(4.1.6)(4.1.7)(4.1.8)得$$\begin{array}{rl}(x_k,y_j{)}^{\ast }& =\mathrm{argmax}\mathcal{N}(h(y_j,x_k),Q_{k,j})\\ & =\mathrm{argmin}\left((z_{k,j}-h(y_j,x_k{)}^T{Q}_{k,j}^{-1}(z_{k,j}-h(y_j,x_k)\right)\end{array}\text{\hspace{1em}(4.1.9)}$$该式子等价于最小化噪声项（即误差）的一个二次型，称该二次型为马哈拉诺比斯距离（Mahalanobis distance），又叫马氏距离。也可以看成由 $Q_{k,j}^{-1}$ （信息矩阵）加权之后的欧氏距离（二范数）。上述为观测方程，运动方程同理可得。
由于各个时刻的输入和观测为相互独立，因此可以联合分布对批量法的数据进行因式分解$$P(z,u|x,y)=\prod _{k}P(u_k|x_{k-1},x_k)\prod _{k,j}P(z_{k,j}|x_k,y_j)\text{\hspace{1em}(4.1.10)}$$定义各次运动方程（输入）和观测方程（观测）的误差$$\begin{array}{r}{e}_{u,k}=x_k-f(x_{k-1},u_k)\\ e_{z,j,k}=z_{k,j}-h(x_k,y_j)\end{array}$$最大似然估计等价于求最小化所有时刻估计值于真实读数之间的马氏距离。目标函数如下$$\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{Q}_{k,j}^{-1}{e}_{z,k,j}\text{\hspace{1em}(4.1.11)}$$就得到了一个最小二乘问题（Least Square Problem）。由于误差的存在，估计的轨迹于地图代入运动、观测方程式并不会完美成立，因此我们采取对状态的估计值进行微调，使得整体误差下降到某一极小值的限度，这就是非线性优化问题。

---

4.2 非线性最小二乘

对于简单的最小二乘表达式$$\underset{x}{\min }F(x)=\frac{1}{2}\Vert f(x){\Vert }_2^2\text{\hspace{1em}(4.2.1)}$$其中，自变量 $x\in {\mathbb{R}}^n$，$f$ 是任意标量非线性函数 $f(x):{\mathbb{R}}^n\to \mathbb{R}$。系数 $\frac{1}{2}$ 可有可无。由于函数 $f(x)$ 为非线性函数，所求得最值无法用线性函数直接求导得出，因此采用迭代的方式，从一个初始值出发，不断的更新当前优化变量，使目标函数下降。具体步骤如下：

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

让求解导函数为零的问题变为了一个不断寻找下降增量 $\Delta x_k$ 的问题。

一阶和二阶梯度法
考虑第 $k$ 次迭代，在 $x_k$ 处寻求增量 $\Delta x_k$，最直观的方式是将目标函数 $F(x)$ 在 $x_k$ 附近进行泰勒展开（线性化处理）$$F(x_k+\Delta x_k)\approx F(x_k)+J(x_k{)}^T\Delta x_k+\frac{1}{2}\Delta x_k^{T}H(x_k)\Delta x_k\text{\hspace{1em}(4.2.2)}$$其中 $J(x_k)$ 是 $F(x)$ 关于 $x$ 的一阶导数，也叫梯度、雅可比（Jacobian）矩阵，$H(x)$ 则是二阶导数，也叫海塞（Hessian）矩阵。保留一阶项和二阶项对应求解方法中的一阶梯度或二阶梯度法。如果保留一阶梯度，取增量为反向的梯度，即可保证函数下降（梯度表示函数在该点处沿着此梯度方向变化最快，变化率最大，变化率值为梯度的模）$$\Delta x^{\ast }=-J(x_k)\text{\hspace{1em}(4.2.3)}$$该式表示下降的方向，通常还需要指定一个可以根据一定条件算出的步长 $\lambda$。一阶梯度法也称为最速下降法（Steepest Method），直观意义为只要沿着反梯度的方向前进，在一阶（线性）的近似下，目标函数必定会下降。可以看出上述列举的第 $k$ 次迭代只与 $k$ 次有关，因此可省略下角标 ${}_k$，适用于任何一次迭代。一阶梯度法是根据梯度的意义求得。
同时，也可选择保留二阶梯度信息，此时增量方程为$$\Delta x^{\ast }=\mathrm{argmin}\left(F(x)+J(x{)}^T\Delta x+\frac{1}{2}\Delta x^{T}H(x)\Delta x\right)$$由于只含有 $\Delta x$ 的一次、二次项，因此对 $\Delta x$ 求导，导数为零可得$$J+H\Delta x=0\Rightarrow H\Delta x=-J\text{\hspace{1em}(4.2.4)}$$求解该线性方程，得到 $\Delta x$ 即可，二阶梯度法又称牛顿法。二阶梯度法是根据求导为零求得。

梯度法是通过对初始值开始迭代，在迭代值处对目标函数（非线性函数）线性化，通过所得线性方程的最小值猜测目标函数的极小值。弊端就是最速法过于贪婪（与梯度值有关），容易出现Zigzag形，反而增加了迭代次数；而牛顿法需要求解目标函数的 $H$ 矩阵，问题规模较大时，难以求解该矩阵，通常避免直接求解 $H$ 矩阵。对于一般的问题，一些拟牛顿法可以得到较好的效果，对于最小二乘问题，有高斯牛顿法和列文伯格—马夸尔特方法。

---

高斯牛顿法
高斯牛顿法是优化算法中最简单的算法之一。它的思想是将式子(4.2.1)中的 $f(x)$ 一阶泰勒展开（注意选取的函数不是目标函数 $F(x)$ ）$$f(x+\Delta x)\approx f(x)+J(x{)}^T\Delta x\text{\hspace{1em}(4.2.5)}$$其中，$J(x)$ 为 $f(x)$ 关于 $x$ 的导数（牛顿法中的 $J$ 和 $H$ 是关于 $F(x)$ 的）。也可以看成$$\underset{f(x)}{\min }F(x)=\frac{1}{2}\Vert f(x){\Vert }_2^2$$区别于牛顿法。
当前的目标是寻求使 $f(x)$ 最小。而对于非线性函数 $f(x)$ 来说，取最小值同理于梯度法。有$$\Delta x^{\ast }=\underset{\Delta x}{\mathrm{argmin}}\frac{1}{2}\Vert f(x)+J(x{)}^T\Delta x{\Vert }^2\text{\hspace{1em}(4.2.6)}$$展开$$\begin{array}{rl}\frac{1}{2}\Vert f(x)+J(x{)}^T\Delta x{\Vert }^2& =\frac{1}{2}{\left(f(x)+J(x{)}^T\Delta x\right)}^T\left(f(x)+J(x{)}^T\Delta x\right)\\ & =\frac{1}{2}\left(\Vert f(x){\Vert }^2+2f(x)J(x{)}^T\Delta x+\Delta x^{T}J(x)J(x{)}^T\Delta x\right)\end{array}$$对 $\Delta x$ 求导，导数为零有$$\begin{array}{rl}& J(x)f(x)+J(x)J(x{)}^T\Delta x=0\\ & \underset{H(x)}{\underbrace{J(x)J(x{)}^T}}\Delta x=\underset{g(x)}{\underbrace{-J(x)f(x)}}\end{array}$$此方程是关于 $\Delta x$ 的线性方程组，称之为增量方程，也叫高斯牛顿方程（Gauss-Newton equation）或者正规方程（Normal equation）。上式还可写为$$H\Delta x=g\text{\hspace{1em}(4.2.7)}$$此式对比式子(4.2.4)牛顿法，高斯牛顿法用 $J{J}^T$ 作为牛顿法中二阶Hessian矩阵的近似，从而省略了 $H$ 的计算过程。高斯牛顿法的算法步骤：

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

为了求解增量方程，需要求解 $H^{-1}$，需要 $H$ 矩阵可逆，但实际中计算的 $J{J}^T$ 多数为半正定，此时增量的稳定性较差，导致算法不收敛。

---

列文伯格—马夸尔特法
此方法在一定程度上，修正了上述的 $H$ 矩阵的问题，但收敛速度可能更慢，被称为阻尼牛顿法（Damped Newton Method）。高斯牛顿法中的泰勒展开只在展开点附近有较好的近似效果，所以很自然的想到应该给 $\Delta x$ 添加一个范围—信赖区域（Trust Region）。该范围定义了在什么区域近似是有效的，该类方法也称为信赖区域方法（Trust Region Method）。
确定信赖区域的一个较好的办法是根据我们的近似模型和实际函数之间的差异来确定：差异小，效果好，可以扩大近似范围；差异大，效果差，可以缩小近似范围。定义一个指标 $\rho$ 来刻画近似程度：$$\rho =\frac{f(x+\Delta x)-f(x)}{J(x{)}^T\Delta x}\text{\hspace{1em}(4.2.8)}$$如果 $\rho$ 比较大，近似效果可以放大；比较小，近似效果需要缩小；接近于1，则近似是好的。
改良版非线性优化框架如下：

• 给定初始值 $x_0$，以及初始优化半径 $\mu$。
• 对于第 $k$ 次迭代，在高斯牛顿法的基础上加上信赖区域：$$\underset{\Delta x_k}{\min }\frac{1}{2}\Vert f(x_k)+J(x_k{)}^T\Delta x_k{\Vert }^{2}s.t.\Vert D\Delta x_k{\Vert }^2\le \mu \text{\hspace{1em}(4.2.9)}$$其中，$\mu$ 是信赖区域的半径，$D$ 为系数矩阵。
• 按照式子(4.2.8)计算 $\rho$。
• 若 $\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 步，否则结束。

框架中的近似范围扩大的倍数和阈值都是经验值，可以替换成别的值；在公式(4.2.9)中，把增量限定于一个半径为 $\mu$ 的球中，若带上系数矩阵 $D$，可以视为一个椭球。
列文伯格提出的优化方法是把 $D$ 取成单位阵 $I$，相当于直接把 $\Delta x_k$ 约束在一个球中；马夸尔特提出的方法是将 $D$ 取成一个非负对角阵（实际上用 $J^{T}J$ 的对角元素平方根，使得在梯度小的维度上约束范围更大一些）。
将式子(4.2.9)通过拉格朗日乘子将约束条件放到目标函数中去，构成拉格朗日函数：$$\mathcal{L}(\Delta x_k,\lambda )=\frac{1}{2}\left(\Vert f(x_k)+J(x_k{)}^T\Delta x_k{\Vert }^2+\lambda \left(\Vert D\Delta x_k{\Vert }^2-\mu \right)\right)\text{\hspace{1em}(4.2.10)}$$其中，$\lambda$ 为拉格朗日乘子。令对 $\Delta x_k$ 的导函数为零，仍为计算增量的线性方程$$(H+\lambda D^{T}D)\Delta x_k=g\text{\hspace{1em}(4.2.11)}$$简化形式为$$(H+\lambda I)\Delta x_k=g\text{\hspace{1em}(4.2.12)}$$参数 $\lambda$ 比较小时， $H$ 占主导位置，说明二阶近似模型在该范围内效果较好，列文伯格—马夸尔特法较为接近高斯牛顿法；当 $\lambda$ 比较大，$\lambda I$ 占主导位置，列文伯格—马夸尔特法较为接近一阶梯度法，说明附近的二阶近似不够好。

---

BA与图优化

BundleAdjustment，是指从视觉图像中提炼出最优的 3D 模型和相机参数（内参和外参）。考虑从任意特征点发射出来的几束光线(bundles of light rays)，它们会在几个相机的成像平面上变成像素或是检测到的特征点。如果我们调整(adjustment)各相机姿态和各特征点的空间位置，使得这些光线最终收束到相机的光心。