视觉SLAM笔记（26） 状态估计问题

---

1. 噪声

在前面介绍了经典 SLAM 模型的运动方程和观测方程
现在已经知道，方程中的位姿可以由变换矩阵来描述，然后用李代数进行优化
观测方程由相机成像模型给出，其中内参是随相机固定的，而外参则是相机的位姿
于是，已经弄清了经典 SLAM 模型在视觉情况下的具体表达

然而，由于噪声的存在，运动方程和观测方程的等式必定不是精确成立的
尽管相机可以非常好地符合针孔模型，但遗憾的是，得到的数据通常是受各种未知噪声影响的
即使有着高精度的相机，运动方程和观测方程也只能近似的成立

所以，与其假设数据必须符合方程
不如来讨论，如何在有噪声的数据中进行准确的状态估计
大多现代视觉 SLAM 算法都不需要那么高成本的传感器
甚至也不需要那么昂贵的处理器来计算这些数据，这全是算法的功劳

由于在 SLAM 问题中，同一个点往往会被一个相机在不同的时间内多次观测，同一个相机在每个时刻观测到的点也不止一个
这些因素交织在一起，拥有了更多的约束，最终能够较好地从噪声数据中恢复出需要的东西

---

2. 噪声影响

回顾一下 视觉SLAM笔记（4） SLAM的数学表述 的经典 SLAM 模型：

它由一个状态方程和一个运动方程构成：

现在了解运动方程这里的 $x$_k 是相机的位姿，可以使用变换矩阵或李代数表示它
至于观测方程，即针孔相机模型

为了有更深的印象，不妨讨论一下它们的具体参数化形式
首先，位姿变量 $x$_k可以由 $T$_k 或 $exp$( $ξk$^) 表达，二者是等价的
由于运动方程在视觉 SLAM 中没有特殊性，暂且不讨论它，主要讨论观测方程

假设在 $x$_k 处对路标 $y$_j 进行了一次观测，对应到图像上的像素位置 $z$_k,j
那么，观测方程可以表示成：

通过 视觉SLAM笔记（20） 单目相机模型 知道这里 K 为相机内参， s 为像素点的距离
同时这里的 $z$_k,j和 $y$_j 都必须以齐次坐标来描述，且中间有一次齐次到非齐次的转换

现在，考虑数据受噪声的影响后，会发生什么改变
在运动和观测方程中，通常假设两个噪声项 $w$_k， $v$_k,j 满足零均值的高斯分布：

在这些噪声的影响下，希望通过带噪声的数据 $z$ 和 $u$，推断位姿 $x$ 和地图 $y$（以及它们的概率分布）
这构成了一个状态估计问题

由于在 SLAM 过程中，这些数据是随着时间逐渐到来的
所以在历史上很长一段时间内，研究者们使用滤波器，尤其是扩展卡尔曼滤波器（EKF）求解它
卡尔曼滤波器关心当前时刻的状态估计 $x$_k，而对之前的状态则不多考虑

相对的，近年来普遍使用的非线性优化方法，使用所有时刻采集到的数据进行状态估计
并被认为优于传统的滤波器，成为当前视觉 SLAM 的主流方法

---

3. 最大后验与最大似然

从概率学角度来看一下正在讨论什么问题
在非线性优化中，把所有待估计的变量放在一个“状态变量”中：

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

类似于 $x$，这里 $u$ 和 $z$ 也是对所有数据的统称
特别地，当没有测量运动的传感器，只有一张张的图像时
即只考虑观测方程带来的数据时，相当于估计 $P(x|z)$ 的条件概率分布

如果忽略图像在时间上的联系，把它们看作一堆彼此没有关系的图片
该问题也称为 Structure from Motion（SfM），即如何从许多图像中重建三维空间结构
在这种情况下， SLAM 可以看作是图像具有时间先后顺序的，需要实时求解一个 SfM 问题

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

叶斯法则左侧 $P(x|z)$ 通常称为 后验概率，右侧的 $P(z|x)$ 称为 似然，另一部分 $P(x)$ 称为 先验

直接求后验分布是困难的，但是求一个状态最优估计
使得在该状态下，后验概率最大化（Maximize a Posterior， MAP），则是可行的：

请注意贝叶斯法则的分母部分与待估计的状态 $x$ 无关，因而可以忽略
贝叶斯法则告诉我们，求解最大后验概率， 相当于最大化似然和先验的乘积

进一步，当然也可以说，不知道机器人位姿大概在什么地方，此时就没有了先验
那么，可以求解 $x$ 的最大似然估计（Maximize Likelihood Estimation, MLE） ：

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

---

4. 最小二乘

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

由于假设了噪声项 $v$_k ∼ $N (0,$ $Q$_k,j $)$，所以观测数据的条件概率为：

它依然是一个高斯分布
为了计算使它最大化的 $x$_k, $y$_j，往往使用 最小化负对数 的方式，来求一个高斯分布的最大似然

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

取它的负对数，则变为：

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

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

并求该误差的平方之和：

这就得到了一个总体意义下的最小二乘问题（Least Square Problem）
明白它的最优解等价于状态的最大似然估计

直观来讲，由于噪声的存在，当把估计的轨迹与地图代入 SLAM 的运动、观测方程中时，它们并不会完美的成立
这时候需要把状态的估计值进行微调，使得整体的误差下降一些
当然这个下降也有限度，它一般会到达一个极小值
这就是一个典型非线性优化的过程

仔细观察上式，发现 SLAM 中的最小二乘问题具有一些特定的结构：

• 整个问题的目标函数由许多个误差的（加权的）平方和组成
  虽然总体的状态变量维数很高，但每个误差项都是简单的，仅与一两个状态变量有关
  例如运动误差只与 $x$_k−1， $x$_k 有关，观测误差只与 $x$_k， $y$_j 有关
  每个误差项是一个小规模的约束，之后会谈论如何对它们进行线性近似
  最后再把这个误差项的小雅可比矩阵块放到整体的雅可比矩阵中
  由于这种做法，称每个误差项对应的优化变量为 参数块（Parameter Block） 。

• 整体误差由很多小型误差项之和组成的问题
  其增量方程的求解会具有一定的稀疏性，使得它们在大规模时亦可求解

• 如果使用李代数表示，则该问题是无约束的最小二乘问题
  但如果用旋转矩阵（变换矩阵）描述位姿，则会引入旋转矩阵自身的约束（旋转矩阵必须是正交阵且行列式为 1）
  额外的约束会使优化变得更困难，这体现了李代数的优势

• 使用了平方形式（二范数）度量误差，它是直观的，相当于欧氏空间中距离的平方
  但它也存在着一些问题，并且不是唯一的度量方式，亦可使用其他的范数构建优化问题

---

参考：

《视觉SLAM十四讲》

---

相关推荐：

视觉SLAM笔记（25） 拼接点云
视觉SLAM笔记（24） 图像基础操作
视觉SLAM笔记（23） 图像
视觉SLAM笔记（22） RGB-D相机模型
视觉SLAM笔记（21） 双目相机模型

---

谢谢！

转载：https://blog.csdn.net/qq_32618327/article/details/102402912