1. 误差累积

前端视觉里程计能给出一个短时间内的轨迹和地图
但由于不可避免的误差累积，这个地图在长时间内是不准确的
所以，在视觉里程计的基础上
还希望构建一个尺度、规模更大的优化问题，以考虑长时间内的最优轨迹和地图
不过，考虑到精度与性能的平衡，实际当中存在着许多不同的做法

2. 状态估计的概率解释

觉里程计只有短暂的记忆，而希望整个运动轨迹在较长时间内都能保持最优的状态
可能会用最新的知识，更新较久远之前的状态——站在“久远的状态”的角度上看，仿佛是未来的信息告诉它“你应该在哪里”
所以，在后端优化中，通常考虑一个更长时间内（或所有时间内）的状态估计问题
而且不仅使用过去的信息更新自己的状态，也会用未来的信息来更新自己
这种处理方式不妨称为“批量的”（Batch）

否则，如果当前的状态只由过去的时刻决定，甚至只由前一个时刻决定
那不妨称为“渐进的”（Incremental）

已经知道 SLAM 过程可以由运动方程和观测方程来描述
那么，假设在 t = 0 到 t = N 的时间内
有 x₀ 到 x_N 那么多个位姿，并且有 y₁,…, y_M 那么多个路标
按照之前的写法，运动和观测方程为：

在这里插入图片描述

注意以下几点：

观测方程中，只有当 x_k 看到了 y_j 时，才会产生观测数据，否则就没有
事实上，在一个位置通常只能看到一小部分路标
而且，由于视觉 SLAM 特征点数量众多
所以实际当中观测方程数量会远远大于运动方程的数量
可能没有测量运动的装置，所以也可能没有运动方程
在这个情况下，有若干种处理方式：认为确实没有运动方程，或假设相机不动，或假设相机匀速运动
这几种方式都是可行的
在没有运动方程的情况下，整个优化问题就只由许多个观测方程组成
这就非常类似于 SfM（Structure from Motion）问题，相当于通过一组图像来恢复运动和结构
与 SfM 中不同的是， SLAM 中的图像有时间上的先后顺序，而SfM 中允许使用完全无关的图像

每个方程都受噪声影响
所以要把这里的位姿 x 和路标 y 看成服从某种概率分布的随机变量，而不是单独的一个数

3. 所关心的问题

因此，关心的问题就变成了：
当拥有某些运动数据 u 和观测数据 z 时，如何来确定状态量 x，y 的分布？

进而，如果得到了新来的数据之后，那么它们的分布又将发生怎样的变化？
在比较常见且合理的情况下，假设状态量和噪声项 服从高斯分布
意味着在程序中，只需要储存它们的均值和协方差矩阵即可
均值可看作是对变量最优值的估计，而协方差矩阵则度量了它的不确定性

那么，问题转变为：
当存在一些运动数据和观测数据时，如何去估计状态量的高斯分布？
依然设身处地地扮演一下小萝卜
只有运动方程时，相当于蒙着眼睛在一个未知的地方走路
尽管知道自己每一步走了多远，但是随着时间增长，将对自己的位置越来越不确定
这说明在输入数据受噪声影响时，对位置方差的估计将越来越大
但是，当睁开眼睛时，由于能够不断地观测到外部场景，使得位置估计的不确定性变小了
如果用椭圆或椭球直观地表达协方差阵，那么这个过程有点像是在手机地图软件中走路的感觉
以下图为例，可以想象，当没有观测数据时，这个圆会随着运动越来越大
而如果有正确观测的话，圆就会缩小至一定的大小，保持稳定

在这里插入图片描述

4. 最大似然估计

上面的过程以比喻的形式解释了状态估计中的问题，下面要以定量的方式来看待它
在视觉SLAM笔记（26）状态估计问题，介绍了最大似然估计，提到把状态估计转换为最小二乘的做法
现在更仔细地讨论这个问题
首先，由于位姿和路标点都是待估计的变量，改变一下记号，令 x_k 为 k 时刻的所有未知量
它包含了当前时刻的相机位姿与 m 个路标点
在这种记号的意义下（虽然与之前稍有不同，但含义是清楚的），写成：
在这里插入图片描述
同时，把 k 时刻的所有观测记作 z_k
于是，运动方程与观测方程的形式可写得更加简洁
这里不会出现 y，但这时 x 中已经包含了之前的 y 了：

现在考虑第 k 时刻的情况
希望用过去 0 到 k 中的数据，来估计现在的状态分布：
在这里插入图片描述

下标 0 : k 表示从 0 时刻到 k 时刻的所有数据
请注意 z_k 来表达所有在 k 时刻的观测数据，注意它可能不止一个，只是这种记法更加方便

下面来看如何对状态进行估计：
按照贝叶斯（Bayes）法则，把 z_k 与 x_k 交换位置，有：
在这里插入图片描述
这里 第一项称为似然，第二项称为先验

似然由观测方程给定，而先验部分，要明白当前状态 x_k 是基于过去所有的状态估计得来的
至少，它会受 x_k−1 影响，于是按照 x_k−1 时刻为条件概率展开：

在这里插入图片描述
如果考虑更久之前的状态，也可以继续对此式进行展开
但现在只关心 k 时刻和 k − 1 时刻的情况
至此，给出了贝叶斯估计，但上式还没有具体的概率分布形式
所以还没法实际地操作它
对这一步的后续处理，方法上产生了一些分歧

大体来说，存在若干种选择：
其一是假设马尔可夫性，简单的一阶马氏性认为， k 时刻状态只与 k −1 时刻状态有关，而与再之前的无关
如果做出这样的假设，就会得到以扩展卡尔曼滤波（EKF）为代表的滤波器方法
在滤波方法中，会从某时刻的状态估计，推导到下一个时刻
另外一种方法是依然考虑 k 时刻状态与之前所有状态的关系，此时将得到非线性优化为主体的优化框架

目前视觉SLAM 主流为非线性优化方法

参考：

《视觉SLAM十四讲》

视觉SLAM笔记（49） 后端状态估计