第10讲 后端2

主要目标：理解滑动窗口法、位姿图优化、带IMU紧耦合的优化、掌握g2o位姿图。

第9讲介绍了以为BA为主的图优化。BA能精确优化每个相机位姿与特征点位置。不过在更大的场景中，大量特征点的存在会严重降低计算效率，导致计算量越来越大，无法实时化。本讲第一部分介绍一种简化的BA：位姿图

目录

0、名词解释

1、滑动窗口滤波与优化

1.1 实际环境下的BA结构

1.2 滑动窗口法

2、位姿图

2.1 位姿图的意义

2.2 位姿图的优化

3、实践：位姿图优化

3.1 g2o原生位姿图

3.2 李代数上的位姿图优化

3.3 小结

---

0、名词解释

• SfM（Structure from Motion）是一种从多个视角拍摄的对象或场景中重建其三维结构的方法。
• VO（Visual Odometry）或VIO（Visual-Inertial Odometry）系统：解决的是里程计问题，里程计部分是由惯导，轮速计，视觉，激光等多传感器融合完成。

1、滑动窗口滤波与优化

1.1 实际环境下的BA结构

• 控制计算规模的方法
  • 从连续的视频中抽出一部分作为关键帧，仅构造关键帧与路标点之间的BA，非关键帧只用于定位，对建图则无贡献。
  • 按照某种原则，取时间上靠近当前时刻，空间上又可以展开的关键帧，从而保证相机即使在停止不动时，BA结构也不至于缩成一团。
  • 定义共视图（与现在的相机存在共同观测的关键帧构成的图）结构，在BA优化时，按照某些原则在共视图内取关键帧和路标优化。例如，仅优化于当前帧有20个共视路标的关键帧，其余部分固定不变。
  • 滑动窗口也好，共视图也好，都是对实时计算的某种工程上的折中。

1.2 滑动窗口法

• 滑动窗口法比较适合VO系统，而不适合大规模建图的系统。
• 在边缘化时，我们考虑关键帧的位姿，即：，其中是第k个关键帧的位姿均值，为所有关键帧的协方差矩阵。均值部分是指BA迭代之后的结果，而就是对整个BA的H矩阵进行边缘化之后的结果，即上一讲的S矩阵。
• 当窗口结构发生改变，这些状态变量如何变化？
  • 需要在窗口新增一个关键帧，以及它观测到的路标点。
  • 需要把窗口中的一个旧的关键帧删除，也可能删除它观测到的路标点。
•  新增一个关键点和路标点
  • 只需按照正常BA流程处理即可，对所有点进行边缘化时，即得到这N+1个关键帧的高斯分布参数。
• 删除一个旧的关键帧
  • 若考虑边缘化旧的关键帧，那么Schur消元过程相当于通过矩阵行和列操作消去非对角线处几个非零矩阵块，显然将导致右下角路标矩阵块不再是非对角矩阵。这个过程称为边缘化中的填入（Fill-in）。这时BA无法按照先前的稀疏方式迭代求解。
  • 若对边缘化过程进行一些改造，也可以保持滑动窗口BA的稀疏性。例如，在边缘化某个旧的关键帧时，同时边缘化它观测到的路标点。这样，路标点的信息就会转换成剩下那些关键帧之间的共视信息，从而保持右下角部分的对角块结构。
  • 还有其他更复杂的边缘化策略，例如OKVIS。
•  SWF中边缘化的直观解释
  • 边缘化在概率上的意义就是指条件概率。所以，我们边缘化某个关键帧，即“保持这个关键帧当前的估计值，求其他状态变量以这个关键帧为条件的条件概率”。
    • 当某个关键帧被边缘化，它观测到的路标点就会产生一个“这些路标应该在哪里”的先验信息，从而影响其余部分的估计值。
    • 如果再边缘化这些路标点，那么它们的观测者将得到一个“观测它们的关键帧应该在哪里”的先验信息。
    • ，然后舍去被边缘化部分的信息。
  •  滑动窗口法比较适合VO系统，而不适合大规模建图系统。
  • g2o和 ceres还未直接支持滑动窗口法中的边缘化操作，这里只讲理论部分。

2、位姿图

2.1 位姿图的意义

• 根据前面的讨论，我们发现特征点在优化问题中占据了绝大部分，实际上，经过若干次观测之后，收敛的特征点位置变换很小，发散的外点则已被剔除。因此更倾向于在优化几次之后就把特征点固定住，只把它们看做位姿估计的约束，不在实际地优化它们的位置估计。
• 因此，完全可以构建一个只有轨迹的图优化，而位姿节点之间的边，可以由两个关键点之间通过特征匹配后的得到的运动估计来给定初始值，之后不再优化那些路标点的位置，只关心所有相机位姿之间的联系。如此，省去大量特征点优化的计算，只保留关键帧的估计，从而构建位姿图（Pose Graph）。
• 如果我们有额外测量Pose的传感器，那么位姿图也是一种常见的融合Pose测量的方法。

2.2 位姿图的优化

• 位姿图优化的节点表示相机位姿，边是两个位姿节点之间相对运动的估计，该估计可以来自于特征点法或直接法，也可以来自GPS或IMU积分。
• 假设我们估计了与之间的一个运动。该运动表达可以是：，或按李群的写法：
• 上述等式不会精确成立，因此我们设立最小二乘误差，讨论误差关于优化变量的导数。误差：
• 求对两个优化变量,的导数。
  • 
  • 
  •  若误差接近0，左右雅可比可以近似为或
• 雅可比求导后，剩下就和普通图优化一样了。
• 综上，所有位姿顶点和位姿-位姿边构成一个图优化，本质上是一个最小二乘问题，优化变量为各个顶点的位姿，边来自于位姿观测约束。记为所有边集合，则总体目标函数：。可以用高斯牛顿法或列文伯格-马夸尔特方法求解此问题。

3、实践：位姿图优化

3.1 g2o原生位姿图

g2o_viewer 在g2o/bin下未找到，未安装成功。安装libqglviewer-dev-qt5 后，重新安装g2o解决。参考以下文档
E: 软件包 libqglviewer-dev 没有可安装候选解决方法-CSDN博客

3.2 李代数上的位姿图优化

Sophus表达李代数，用到g2o中定义顶点和边。

运行代码时，误差大于书中描述误差，暂未查出具体原因？

3.3 小结

• PTAM（Parallel Tracking and Mapping）提出以来，人们意识到后端优化没必要实时响应前端的图像数据，人们倾向于把前端后端分开，运行于两个独立线程之中，称为跟踪和建图。前端需要实时响应视频的速度，而优化不必，只要在优化完成时把结果返回前端即可。所有通常不会对后端提出很高的速度要求。