视觉SLAM十四讲-第十一讲笔记

后端优化主要有两种方法，一是以BA为主的图优化，另一种是使用位姿图的优化方法。本讲将讲述位姿图的优化方法。

一、 位姿图介绍

后端优化的目标是在前端给出位姿和路标的初始估计值后，使用已得的全部数据对其进行优化。其中特点是：

• 特征点变量数远大于位姿变量数
• 特征点变量会趋于稳定

其中特征点变量在几次优化后的情况分为两种：

• 收敛：其空间位置会收敛至一个值（三维坐标）保持不动。
• 发散：称为外点，处于视野外，看不到，不再计入地图。

因此，高效的后端优化只需要做：

• 只优化位姿变量
• 无需与前端关联。

而BA做的是：优化每一个相机的位姿和每一个特征点的位置。这样会导致在特征点数目很多的大场景、或运行时间变长后，计算量过大，以至于无法实时化。
因此可以构建位姿图，其特点是：

• 顶点：优化变量只有位姿。$\epsilon$
• 边：运动方程，两个为子节点之间的相对运动估计$\delta {\epsilon }_{i,j}$。由两个关键字之间通过特征匹配之后得到的运动估计来给定初始值，为什么边有初始值?

二、 位姿图优化

与图优化一样，只不过建的图只用位姿不用特征点。思路也是，确定边、顶点，建图，确定误差，确定目标函数，计算雅可比，转为最小二乘问题，使用使用GN、LM等方法优化。

• 顶点：位姿变量（初值由前端给出，李代数标识)
• 边：相邻位姿节点之间的运动估计
• 误差：$e_{i,j}$，对${\epsilon }_i$，${\epsilon }_j$分别求导
• 雅可比：给李代数优化变量左扰动，使用BCH近似导出
• 目标函数：min对于所有边的最小二乘误差

三、 因子图优化

1. 贝叶斯网络

贝叶斯网络是一种概率图：

• 是有向无环图
• 节点：随机变量。SLAM中是位姿、路标点、观测、运动读数
• 边：表达随机变量的独立性。SLAM中是运动方程和观测方程表达的变量条件概率（依赖关系）。
• 优化目标：调整随机变量的取值，max 后验概率。
  $$x,l\ast =argmax(x_0)\prod P(x_k|x_{k-1},u_k)\prod P(z_k|x_i,l_j)$$

2. 因子图

贝叶斯网络可以转化成因子图：

• 是无向图
• 存在两种节点:
  • 变量节点：表示待优化变量。在SLAM里是位姿和路标。
  • 因子节点：表示因子，即待优化变量之间的关系。SLAM中来自运动方程和观测方程。
• 优化目标：调整变量的值，使因子乘积最大化。

优化方法:
将因子表示为高斯分布：
$$f_k:P(x_k|x_{k-1})=N(f(x_{k-1},u_k),R_k)$$
$$z_k:P(z_{k,j}|x_k,l_j)=N(h(x_k,l_j),Q_{k,j})$$
其中均值来自运动方程和观测方程，协方差表示了其不确定性。

调整$x$，$l$，最大化这些高斯概率的乘积。可转化成最小二乘问题求解。
同时可以加入其他因子，如先验因子、回环因子等。

3. 增量特性

使用近似以减少计算：近似认为加入新节点时，只有最后一个与之相连的节点受影响；发生闭环时，只有闭环路径的节点受影响。仅更新这些节点。