深蓝-视觉slam-第七讲学习笔记

思考之路

已于 2022-06-27 23:00:19 修改

阅读量605

点赞数 2

分类专栏：深蓝orb-slam 文章标签： slam

于 2022-04-25 20:37:08 首次发布

本文链接：https://blog.csdn.net/weixin_46417419/article/details/124295837

版权

深蓝orb-slam 专栏收录该内容

22 篇文章 18 订阅

订阅专栏

通过前面学的知识，我们了解到视觉里程计能得到短时间内的轨迹和地图，但由于不可避免的误差累积，这个地图在长时间内是不准确的。所以在视觉里程计的基础上，我们需构建一个尺度，规模更大的优化问题，来考虑长时间内最优的轨迹和地图。

滤波器为主的后端
非线性优化为主的后端
在这里插入图片描述

假设了马尔科夫性(k时刻的状态只与k-1时刻的状态有关——扩展卡尔曼滤波器（EKF)为代表的滤波方法；
k时刻的状态与之前所有的姿态都有关——以非线性优化为主体的优化框架；
1.线性系统和KF
在这里插入图片描述

线性KF——知道k-1时刻的后验状态估计以及协方差，根据k时刻的输入和观测数据来确定k时刻的后验分布。

预测：通过上一时刻的状态，根据当前的输入信息(带噪声)来推断当前时刻的分布，这是当前时刻的先验部分。
先验的状态的不确定性会增大(添加了噪声),通过观测，我们计算在某个状态下应该产生怎样的观测数据。将预测与当前的观测相乘
更新：预测的值通过更新后的到得到最优的位姿和路标状态。

2.非线性系统和EKF
在SLAM中的运动方程和观测方程通常都是非线性化的，尤其是SLAM中的相机模型(观测数据),需要使用相机内参模型和李代数表示的位姿。一个高斯系统通过非线性变换后，不再是高斯分布，所以我们在非线性系统中，必须取一定的近似，将一个非高斯分布近似成高斯分布，通常的做法就是在某个点的附近考虑运动方程和观测方程的一阶泰勒展开，只留一阶项，然后安照线性系统推导。
在知道k-1时刻的均值和协方差矩阵后，在k时刻把运动方程和观方程在k-1的值处进行线性化。
在这里插入图片描述

卡尔曼滤波器给出了在线性化之后状态变量分布的变化过程。在线性系统和高斯噪声下，卡尔曼滤波器给出了无偏最优估计。在非线性系统下给出了单次线性近似下的最大后验估计。

EKF(滤波器)和非线性优化的比较：

滤波器假设了马尔科夫性，而非线性优化使用了所有的历史数据，在这中意义下，非线性优化方法使用了更多的信息，也就有更多的计算量。
EKF滤波器仅在根据k-1时刻的状态做了一次线性优化，直接根据这次的优化结构把k时刻的后验变量的分布给算了出来。这就相当与说，该点处的线性优化的近似在后验概率处仍然是有效的。但实际上，一阶泰勒展开并不一定能过近似整个函数，这个取决于运动模型和观测模型的非线性情况. EKF的非线性误差：如果他们有很强烈的非线性，那么只能在小范围内线性近似成立，不能认为在很远的地方也成立。在优化问题中，我们也做一阶(最速下降法) 或二阶(GN,LM)的近似，但每迭代一次，状态估计发生变化后，我们会对新的估计再次进行泰勒展开，而不像EKF那样只在固定的点处做一次展开，EKF像是优化过程中的一次迭代。
EKF需要存储状态量的均值和方差，并对他们进行更新。如果把路标也放进来，存储数量很大，且与状态量呈平方增长(因为要存储协方差矩阵),因此 EKF SLAM不适合大场景，使用与计算量有限，待估计量比较简单的场合。
EKF滤波器没有异常检测机制，导致系统很容易发散。由于特征匹配还是光流法，都容易追踪或匹配到错误的点。没有异常检测机制会让系统在实用中非常不稳定。

在同等计算量的情况下，非线性优化能取得更好(精度和鲁棒性)的效果。
3 BA和图优化
BA(只有观测方程)：从视觉图像中提炼出最优的3D模型的相机参数（内参和外参）。调整相机的位姿和各特征点的空间位置，使得这些光线最终收束到相机的光心。
在这里插入图片描述

在这里插入图片描述
BA的求解，我们知道观测模型 $h(x_i, y_j)$ 不是线性函数，我们需要通过一些非线性优化方法将其优化，非线性优化的思路：从某个初始值开始，不断寻找找下降方向 $\Delta x$ ,来找目标函数最优解，不断求增量方程中的增量。在整体BA目标函数上，我们把自变量定义成待优化的变量(路标点和位姿)
在这里插入图片描述
相应的，增量方程中的 $\Delta x$ 则是对整体自变量的增量。

$F_{ij}$ 表示整个代价函数在当前状态下对相机位姿的偏导数，而 $E_{ij}$ 表示i该函数对路标点位置的偏导数。

直接对H求逆算增量运算太大，利用H的特殊结构，可以加速求解

在这里插入图片描述

在这里插入图片描述

邻接矩阵：H矩阵非对角线的非零矩阵，则其位置所对应的变量之间在图中会有一条边，可以理解为两个变量之间存在联系，称之为约束。图优化结构和增量方程的稀疏性存在者明显的联系。

相比与直接求H的逆，通过H的特殊结构求增量的优势：
在这里插入图片描述

在这里插入图片描述
通过边缘化掉 $\Delta x_p$ 后，此方程的系数记为S，S矩阵的稀疏状态如下所式：

H矩阵的非对角块处的非零元素对应着相机和路标的关联。进行消元或边缘化路标点之后S矩阵的非对角线上的非零矩阵块表示了对应的两个相机变量之间存在着共视的路标点，有是称为共视. 如果该快为0，则表明这两个相机变量没有共视观测。
实践中，如ORB-SLAM中的Local Mapping环节，在做BA的时候，可以选择共视观测的帧作为关键帧, 高斯消元后可以得到稠密的矩阵S，由于这个不是实时的，这种做法还可形。
DSO，OKVIS采用了滑动窗口方法，对每一帧做一次BA防止误差的累积，因此采用了一些技巧保持了S矩阵的稀疏性。

鲁棒核函数
在BA优化中，将最小化误差项的二范数平方和作为目标函数，误差很大时，二范数增长快，核函数的存在保证了每条边的误差不会大得可以掩盖其他的边。
具体做法：把原先误差的二范数度量替换成一个增长没那么快的函数，同时保证光滑性质。因为他们使得整个优化结果更加稳健，所以称为鲁棒核函数。
在这里插入图片描述

在这里插入图片描述

前面的部分我们主要讲了以BA为主的图优化，能精确的优化每个相机的位姿与特征点的位置。不过在大的场景中，大量特征点的存在会严重降低计算效率，导致计算量增大，以至于无法实时化，使用一种简化的BA：位姿图可以解决。

带有相机位姿和空间点的图优化称为BA，计算量大且实时性不好。引入滑动窗口
滑动窗口:保留离当前时刻最近的N个关键帧，去掉时间上更早的关键帧，BA将被规定在一个时间窗口内，离开这个窗口被丢弃.取这N个关键帧的具体方法，可以取时间上靠近，空间上又可以展开的关键帧，从而保证相机不动时不至于缩成一团(退化)。当共视图关系能过正确构造的时候，基于共视图的优化也会在更长时间内保持最优。

在这里插入图片描述

从前面的边缘化知道，我们当时边缘化的是路标点，Fill-in将出现在左上角的姿态块中。不过BA中不要求姿态块是对角块，所以稀疏BA求解仍然可行。但是当边缘化关键帧时，将破坏右下角的路标点之间的对角块，这时BA就无法按之前的稀疏方式求解。

在边缘化某个关键帧(相机位姿)时，同时边缘化掉它观测到的路标点。这样路标点的信息就会转换为剩下那些关键帧之间的共视信息，从而保持右下角的对角块结构。例如：OKVIS中，我们要判断那个边缘化的关键帧，他看到的路标点是否在最新的关键帧中能看到，若不能，直接边缘化这个路标点。若能，就丢弃被边缘化帧对这个路标点的观测，从而保持BA的稀疏性。
在这里插入图片描述
位姿图：构建了一个只有轨迹的图优化，而位姿节点之间的边，可以由两个关键帧之间通过特征匹配之后得到的运动估计来给定初始值。一单初始化完成，就不再优化那些路标点的位置，而只关心所有的相机位姿之间的联系。通过这种方式，我们省去了大量计算特征点优化的计算，只保留了关键帧的轨迹，从而构建了位姿图。
在这里插入图片描述