SLAM学习笔记-------------（10）后端2

滑动窗口优化

这个比较好理解。

实际工程中，我们必须控制后端优化的计算规模

控制计算规模的做法有很多，比如从连续的视频中抽出一部分作为关键帧，仅构造关键帧与路标点之间的BA问题，于是非关键帧只用于定位，对建图则没有贡献。

即便如此随着时间的流逝，关键帧数量会越来越多，地图规模也将不断增长。像BA这样的批量优化方法，计算效率会不断下降。

为了避免这种情况，我们需要用一定手段控制后端BA的规模。这些手段可以是理论上的,也可以是工程上的。

例如，最简单的控制BA规模的思路，是仅保留离当前时刻最近的N个关键帧，去掉时间上更早的关键帧。

于是，我们的BA将被固定在一个时间窗口内，离开这个窗口的则被丢弃。这种方法称为滑动窗口法。

当然，取这N个关键帧的具体方法可以有一些改变

例如，不见得必须取时间上最近的，而可以按照某种原则，取时间上靠近，空间上又可以展开的关键帧，从而保证相机即使在停止不动时，BA的结构也不至于缩成一团(这容易导致一些糟糕的退化情况)。

在滑动窗口中，当窗口结构发生改变,这些状态变量应该如何变化?这件事情可以分成两部分讨论:

1、我们需要在窗口中新增一个关键帧,以及它观测到的路标点。
2、我们需要把窗口中一个旧的关键帧删除，也可能删除它观测到的路标点。

新增还好，就直接新增。

删除的话就麻烦一些：

位姿优化

我们可以构建一个只有轨迹的图优化，而位姿节点之间的边，可以由两个关键帧之间通过特征匹配之后得到的运动估计来给定初始值。

不同的是，一旦初始估计完成,我们就不再优化那些路标点的位置，而只关心所有的相机位姿之间的联系。

通过这种方式，我们省去了大量的特征点优化的计算，只保留了关键帧的轨迹，从而构建了所谓的位姿图( Pose Graph )

所以这是两种控制计算规模的方法。

位姿图的优化，数学推导可以看书

总之，最后得到了一个目标函数

我们依然可以用高斯牛顿法、列文伯格—马夸尔特方法等求解此问题，除了用李代数表示优化位姿，别的都是相似的。

根据先前的经验，可以用Ceres或g2o进行求解。我们不再讨论优化的详细过程,第9讲已经讲清楚了。

代码实践：

运行程序pose_graph_g2o_SE3.cpp

首先，预先生成了一个仿真位姿图sphere.g2o

我们可以用g2o_viewer打开这个图（如何操作，请参考：如何找到g2o_viewer并使用）

然后运行代码

这个代码需要传入参数，就是那个图sphere.g2o

执行结果

这个程序会对位姿图进行优化（使用列文伯格-马夸尔特方法）

本来有噪声影响，位姿图是这个样子的

优化结果存储在result.g2o中

打开看下，优化好的样子。一个球

运行程序pose_graph_lie_algebra.cpp

这个程序使用李代数表达位姿图，节点和边的方式为自定义

还是传入参数sphere.g2o

执行结果

优化结果存在result_lie.g2o里面

打开看看