第十一讲 后端2

滑动窗口优化的概念和方法

SLAM系统为了提高精度，会对前端里程计送过来的数据进行BA优化以提高辨识精度。随着系统的运行，后端收到的位姿和路标数量会越来越多，BA优化所需要的时间也会越来越长。为了保证系统的正常运行，就需要对BA的规模进行控制。其中一种有效的办法是滑动窗口法，其主要思想是仅保留离当前时刻最近的N帧进行BA优化。这N帧称为关键帧，其选择方法可以作一些改进。例如它们不仅要时间上比较新，空间上又有一定的间距以免BA结构缩成一团。关键帧的选取应当使得它们与当前帧有较多的共视路标以在BA优化中充分利用路标信息。

滑动窗口法：保留当前时刻最近的N个关键帧，但在滑动过程中，窗口结构发生变化，主要问题在于如何处理新增关键帧和删除关键帧，不同的SLAM框架对该问题具有不同的做法。

位姿图优化

BA过程中，会发现路标点位置在迭代几次后，基本不发生变化，所以可以在优化几次后，不再优化，仅仅将它们当作相机位姿估计的约束。由此，可仅对轨迹进行优化，位姿节点之间的边表示两个相机的相对运动，即位姿图。

位姿图的节点表示相机位姿，边表示两个位姿节点之间的相对运动的估计，该估计可以来自于特征点法或直接法，也可以来自GPS或IMU预积分。

和之间的一个运动：

最小二乘误差：

求误差关于和 的导数，用到了左扰动：

利用到了伴随性质，最后求得：