SLAM基础（三） --图优化

滤波器的缺点：

EKF

SLAM不仅要维护自身的状态，还需要维护地图（特征）
于是必须在内存上做出牺牲，比如500个特征，每个特征在二维环境中是两个点（x轴、y轴），矩阵变为1000x1000，若是三维环境，则1500x1500，状态的方差矩阵规模会变得十分巨大，对计算效益要求很高

粒子滤波器：

随机撒点（每个点都相当于一个机器人），通过landmark和传感器观测数据来更新点，最后收敛。

但是粒子滤波器存在这收敛过慢和例子退化的问题：在大场景中，需要很多次才可以达到收敛，而且要撒非常多的点，计算量巨大；在特征重复的场景中（如一排一模一样的房间），粒子滤波器面临无法收敛的情况；在收敛跟丢之后退化的粒子不能被重新利用起来

回路检测

在相机运动的过程中，会产生误差，滤波器只是在滤波，只能滤掉高斯白噪声等或通过联合矩阵等抑制误差，并不能消除误差干扰。
EKF滤波器是增量计算，矩阵只会保留临近时间的特征，历史图片的特征已经不在矩阵中了，因此无法完成回路检测；同样粒子滤波器是随机撒点，保留的只是位置信息，但是判断是否完成回路（这个地方曾经有没有来过），不是依靠位置信息（兜了一圈之后有误差，位置信息可能是错的，基于位置信息就是基于错误的信息来更新，结果一定是错的），而是通过特征信息来判断

基于视觉的单步定位：

图优化

图SLAM 和 EKF SLAM都是先进行预测，再进行估计，只是估计的方法有所不同：EKF是提取特征，投影到相机平面，再通过卡尔曼做更新；图SLAM是基于预测到的位置再估计观测的特征的位置，再和地图上的这个特征点的位置作比较（一般不重合），要么估计有问题，要么地图有问题，要么都有问题
预测的特征点的位置和地图上的特征点的位置做差，优化预测的相机位置x，令差值最小（Ω是噪声，代表权重，噪声越大，权重应该越小）

图是怎么回事？

landmark、相机状态作为顶点，可见链接为边（相机1能看见landmark1，所以相机1和landmark1有边；相机2能看见landmark1，所以相机2和landmark1有边；相机1和相机2都能看见landmark1，相机1和相机2有边），共同看到的特征点越多，变得权重越大。

Covisibility Graph

定义：由一系列相互可见的标志点和状态组成的无向链接

局部矫正：在很小的范围内（几帧）进行实时矫正

全局矫正：

Marginlization

存在的问题：

假设特征点(Y)和位姿(X)之间的转换关系为G，位姿与位姿之间的转换关系为H，则

1. 随着机器人走的越远，位姿越多，矩阵会变得非常巨大
2. 随着机器人走的越远，前面的状态和特征与后面的状态和特征没有任何关系，导致矩阵0十分多，导致关系矩阵很离散，可供计算的信息很少
   于是为了简化表达，提出了Marginlization
   
   把A除掉，D变成了A和D的融合
   在相机运动过程中：
   
   运算过程：
   得到：
   想要扔掉状态a的信息来简化矩阵，但是又担心会降低约束，通过schur complement的方式将a的信息融合到b中，就可以去掉a了。

要去掉x0，通过schur complement的方式将x0融合到f1中，生成新的f1‘，这就是Marginlization的过程：
要去掉x0，先把x0的pose信息移到矩阵A的位置，再schur complement，降低了矩阵维度的同时，还令矩阵更加dense：