《视觉SLAM14讲》学习小结——供小白快速入门ch10

后端2

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

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

为了实现上述功能，需要明确数据进入和退出滑窗的算法。上一节的分析表明，BA优化的关键是充分利用雅可比矩阵的稀疏性和矩阵的结构特性。这种结构特性是根据相机位姿-路标观测示意图，即ch9中的图1建立的。为了方便阅读，这里把图拷贝如下

图1：相机位姿与路标的观测关系图

这种示意图明确了相机位姿与路标之间的观测关系，在滑窗过程中应当得到保持。当从滑窗中删除一帧相机位时，其单独观测到的路标也应当被删除以减小滑窗中的路标数量。例如ch9图1中删除相机位时也要删除。当往滑窗中添加一帧数据时，需要建立新增路标与原相机位的共视关系。仍如ch9图1所示，假设原来滑窗中只有以及其所观测到的路标。现在里程计新送过来相机位姿信息和路标信息，我们需要明确和是对应点以建立正确的观测关系。这种对应关系的建立对BA优化结果有重要的影响，因原著中未涉及这里略去。其中一种比较简单的处理方法是对比新路标与滑窗中原路标的位置和特征关系，以此判断它们是否属于同一点。如果是的话保留滑窗中旧路标即，删除新路标并连接与的观测关系。有了这种观测关系，就可以在利用矩阵结构特性的基础上进行BA优化。

二、位姿图优化

位姿图优化的思想是仅对位姿进行优化，这种优化并不涉及它与路标的观测关系。位姿图中的点表征相机位姿，以来表示，边则是表示两个位姿节点之间的相对运动，用表示从到的运动。可根据两帧图像中特征点的匹配关系来建立，也可以根据传感器（如IMU惯导器件积分）测量得到。位姿图优化的目的就是为了有效融合这些量测结果以获得最佳的相机位估计。为了实现这个目标，我们首先需要建立优化模型。

根据前面章节的介绍，优化是将计算值与测量值的差作为目标函数，通过调整优化参数使得目标函数取得极小值的过程。这里的优化参数就是相机的位姿和等，测量值就是传感器的观测结果，而相机位到差异的计算值为。这样观测模型为。注意到位姿群不满足加法运算，因此引入李代数可得误差函数为

                                              (1)

上式的含义是，将误差定义为李代数的观测值和计算值之差，它可根据相机位和的值以及相机位变化量的观测值计算得到。优化的目的就是通过调整和的值使得取最小值。我们已经知道进行这种优化的重点是计算误差函数相对优化变量的雅可比矩阵，即。认真学过ch4的读者应该会马上指出李群不满足加法运算，不能直接写。我们在此强调它只是一个形式符号，目的是为了方便读者理解，在实际应用中只能对李导数求导。为了区分是直接求李导数还是扰动模型，还引入变量。这样雅可比矩阵应改写为。下面重点讨论如何计算这两个矩阵。

当相机位即优化变量和分别产生一个微小变化时，相当于对其分别左乘一个矩阵和。这对应着它们的李导数和分别产生一个变化和，这时误差变成了且

                                      (2)

这个式子的特点是三个不在一起，中间有李代数隔离。我们希望三个能写在一起。利用原著中的公式（4.55）并稍加处理可得

                                            (3)

其中作用在矩阵上表示求伴随阵。根据这个式子，式（2）中的可改写为

                 (4)

记

                              (5)

则根据原著中的BCH近似公式（4.30）可将式（4）改写为

                                      (6)

根据式（1），式（5）可改写成

                                  (7)

再次利用原著中的BCH近似公式（4.30）可将上式改写为

                                         (8)

合并式（6）和（8），同时注意到为小量可得

                   (9)

这样对比可知

                       (10)

上式表明，这个结论无疑是正确的，也验证了式（10）的正确性。

上面只是计算了所对应的雅可比矩阵。整个雅可比矩阵需要针对所有的i,j开展并组合成大矩阵。有了这些矩阵，就可以采用现成的软件如g2o对位姿进行优化，具体细节将在程序分析系列讨论。

三、总结

至此我们已经全部完成了后端的内容讲解。回顾前面的内容可以发现SLAM系统包括传感器信息读取（对应ch5），前端视觉里程计（对应ch7,ch8），后端非线性优化（对应ch6,ch9,ch10）。接下来两章我们将讨论回环检测和建图。这些内容构成了SLAM的总体框架。本篇讨论了如下两个内容

（1）滑动窗口优化的概念，其作用是为了限制后端BA优化的规模。然而笔者无法理解原著中的图10-2和相关内容，因此只是根据自己的理解写了一些简要的内容。有懂行的读者请不吝指教。

（2）对于位姿图优化章节，原著存在一些笔误，内容阐述的比较紧凑，初学者特别是不熟悉李导数的读者理解起来比较费力。笔者在此对各个符号给出了更清晰的定义，同时更详细地展示了推导过程，希望有助于读者理解。