视觉slam-----第三篇 最小二乘法

视觉slam系列更多偏向对本人的记录而非讲解，因此叙述部分会占多数，推导过程几乎没有，文章主要内容摘自《视觉slam十四讲》《计算机视觉中的多视图几何》《机器人学中的状态估计》，博客内容仅为帮助自己记忆，本人仅起到总结作用，所有内容均摘自以上几本书。

slam和最小二乘法

1.slam问题中的观测方程与观测噪声

为什么是这个标题，主要是因为视觉slam中常用的g2o实在理解不能，数学水平不够，不说推导了，看懂都难。但是本人以前是做无人机算法的，无人机姿态解算常用算法----粒子滤波和卡尔曼滤波还算熟悉，本篇仅仅写到最小二乘法。

最小二乘法与slam有什么关系呢，那是因为slam问题本质上是通过带噪声的数据最终求得相机位置姿态的过程，是一个优化的问题。一般来说，一个slam模型由一个运动方程和观测方程构成（当然，如果slam问题仅仅包含一个相机，没有里程计，惯导等传感器，这个slam问题就只有观测方程）：

${\mathbf{x}}_k=f\left({\mathbf{x}}_{k-1},{\mathbf{u}}_k\right)+{\mathbf{w}}_k$
${\mathbf{z}}_{k,j}=h\left({\mathbf{y}}_j,{\mathbf{x}}_k\right)+{\mathbf{v}}_{k,j}$

这里的 ${\mathbf{x}}_k$是相机的位姿。我们可以使用变换矩阵或李代数表示它，至于观测方程就是针孔相机模型。在slam问题中，我们暂时可以不用管运动方程。

观测数据受噪声影响，会发生改变。在运动和观测方程中，我们通常假设两个噪声项${\mathbf{w}}_k,{\mathbf{v}}_{k,j}$满足零均值的高斯分布：

${\mathbf{w}}_k\sim N\left(0,{\mathbf{R}}_k\right),{\mathbf{v}}_k\sim N\left(0,{\mathbf{Q}}_{k,j}\right)$

2.slam问题和最大似然估计

如何对带噪声的slam问题进行优化，显然，slam观测方程没有让我们入手的点，这个时候就需要贝叶斯概率了。

对机器人状态的估计，就是求已知输入数据 $\mathbf{u}$ 和观测数据 $z$ 的条件下，对机器人状态算状态 $x$ 的条件概率分布：

$P(\mathbf{x}|\mathbf{z},\mathbf{u})$

类似于 $x$ ，这里 $\mathbf{u}$和 $z$ 也是对所有数据的统称。特别地，当我们没有测量运动的传感器，只有一张张的图像时，即只考虑观测方程带来的数据时，相当于估计 $P(\mathbf{x}|\mathbf{z})$ 的条件概率分布。如果忽略图像在时间上的联系，把它们看作一堆彼此没有关系的图片，该问题也称为 Structure from Motion（SfM），即如何从许多图像中重建三维空间结构 。在这种情况下，SLAM 可以看作是图像具有时间先后顺序的，需要实时求解一个 SfM 问题。为了估计状态变量的条件分布，利用贝叶斯法则，有：

$P(\mathbf{x}|\mathbf{z})=\frac{P(\mathbf{z}|\mathbf{x})P(\mathbf{x})}{P(\mathbf{z})}\propto P(\mathbf{z}|\mathbf{x})P(\mathbf{x})$

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

${\mathbf{x}}_{MAP}^{\ast }=\arg \max P(\mathbf{x}|\mathbf{z})=\arg \max P(\mathbf{z}|\mathbf{x})P(\mathbf{x})$

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

${\mathbf{x}}^{\ast }\mathrm{MLE}=\arg \max P(\mathbf{z}|\mathbf{x})$

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

现在，slam问题被转化为了最大似然估计问题，我们已经知道了观测数据（相机图像），根据最大似然估计，我们需要知道的就是对应当前观测数据的状态，也就是相机的位姿。

3.最小二乘法

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

$z_{k,j}=h\left({\mathbf{y}}_j,{\mathbf{x}}_k\right)+{\mathbf{v}}_{k,j}$

由于我们假设了噪声项 ${\mathbf{v}}_k\sim N\left(0,{\mathbf{Q}}_{k,j}\right)$，所以观测数据的条件概率为：

$P\left({\mathbf{z}}_{j,k}|{\mathbf{x}}_k,{\mathbf{y}}_j\right)=N\left(h\left({\mathbf{y}}_j,{\mathbf{x}}_k\right),{\mathbf{Q}}_{k,j}\right)$

它依然是一个高斯分布。为了计算使它最大化的${\mathbf{x}}_{\mathbf{k}},{\mathbf{y}}_{\mathbf{j}}$，我们往往使用最小化负对数的方式，来求一个高斯分布的最大似然。

高斯分布在负对数下有较好的数学形式。考虑一个任意的高维高斯分布$\mathbf{x}\sim N(\mathbf{\mu },\mathbf{\Sigma })$，它的概率密度函数展开形式为:

$P(\mathbf{x})=\frac{1}{\sqrt{(2\pi {)}^N\det (\mathbf{\Sigma })}}\exp \left(-\frac{1}{2}(\mathbf{x}-\mathbf{\mu }{)}^T{\mathbf{\Sigma }}^{-1}(\mathbf{x}-\mathbf{\mu })\right)$

取它的负对数，则变为：

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

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

${\mathbf{x}}^{\ast }=\arg \min \left({\left({\mathbf{z}}_{k,j}-h\left({\mathbf{x}}_k,{\mathbf{y}}_j\right)\right)}^T{\mathbf{Q}}_{k,j}^{-1}\left({\mathbf{z}}_{k,j}-h\left({\mathbf{x}}_k,{\mathbf{y}}_j\right)\right)\right)$

我们发现，该式等价于最小化噪声项（即误差）的平方（Σ 范数意义下）。因此，对于所有的运动和任意的观测，我们定义数据与估计值之间的误差：

$e_{v,k}=x_k-f\left(x_{k-1},u_k\right)$
$e_{y,j,k}=z_{k,j}-h\left(x_k,y_j\right)$

并求该误差的平方之和：

$J(\mathbf{x})={\sum }_k{\mathbf{e}}_{v,k}^T{\mathbf{R}}_k^{-1}{\mathbf{e}}_{v,k}+{\sum }_k{\sum }_j{\mathbf{e}}_{y,k,j}^T{\mathbf{Q}}_{k,j}^{-1}{\mathbf{e}}_{y,k,j}$

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