【视觉SLAM入门】7.4.后端优化 --- 基于位姿图和基于因子图

引入：

• 上节BA将位姿和路标都作为优化的节点，H矩阵也告诉我们路标远大于位姿，时长会导致资源等问题。

办法：

1. 滑动窗口法：保持H固定大小，丢弃历史
2. 位姿图：不管路标，只管位姿，从BA变成Pose Graph，容易看出资源会少很多很多，如下：
   

1. 位姿图

1.1 具体做法

其实仍然是用构建非线性最小二乘问题然后优化解的，只是优化变量变成了位姿 $\xi$ 。

• 假设，位姿 ${\xi }_i$ 经过运动 $\Delta {\xi }_{ij}$ 变到了 ${\xi }_j$ ，根据李群李代数知识，有如下等式：

$$\Delta {\xi }_{ij}={\xi }_i^{-1}\circ {\xi }_j=ln(exp(-{\xi }_i)\hat{}exp({\xi }_j\hat{}))\stackrel{ˇ}{}$$

用李群表示则如下：

$$\Delta T_{ij}=T_i^{-1}{T}_j$$

将 $T_{ij}$ 右移：

求它关于优化变量 ${\xi }_i$ 和 ${\xi }_j$ 的导数，按照李群李代数的方法，中间过程略，最终构建的总体目标函数如下：

$$\underset{\xi }{\min }\frac{1}{2}\sum _{i,j\in \epsilon }{e}_{ij}^T{\Sigma }_{ij}^{-1}{e}_{ij}$$

这里的 $\epsilon$ 就是所有边的集合，二范数含义就是平方项。关于这个问题的求解，我们可以用G-N,L-M等之前用的很多。略。

1.2 小结

自从PTAM提出来以后，后端优化没必要实时性了；人们将前后端分开作两个线程–跟踪和建图。
前段需要实时响应视频速度，如每秒30HZ，而后端优化只要完成后将结果返回给前端即可，所以实时性没必要了。

2. 因子图

2.1 具体做法

在介绍因子图的做法前，要先了解贝叶斯网络。

2.1.1 贝叶斯网络

直接用一个动态的贝叶斯网络来表达我们的SLAM的运动和观测方程：

• $x$ 表示位姿节点
• $u$ 输入量节点
• $l$ 路标节点
• $z$ 观测数据节点
• 箭头表示依赖关系，比如 $x_2$ 在运动方程中就依赖于 $u_2$ 和 $x_1$， 图中也是这么标注的。
• 一次观测或者一次运动实际上是给出的条件概率关系：

$$P(x_3|x_2,u_3)P(z_1|x_1,l_1)$$

图绘制完，我们后端优化的目标就是—不断调整贝叶斯网络中随机变量的取值，使得整个网络的后验概率最大化：

{ x , l } ∗ = a r g m a x ( x 0 ) ∏ P ( x k ∣ x k − 1 , u k ) ∏ P ( z k ∣ x i , l j ) \{x,l\}^* = arg max(x_0) \prod P(x_k|x_{k-1}, u_k) \prod P(z_k|x_i, l_j) {x,l}∗=argmax(x0​)∏P(xk​∣xk−1​,uk​)∏P(zk​∣xi​,lj​)

---

我们发现要做这个公式，里边的乘积会很多，所以将因子化为节点，会更直观，就得到了—因子图

2.1.2 因子图

根据上边的公式，可以重新结合公式和概率公式绘制网络，得到因子图：

• 圆圈： 变量节点
• 方块： 因子节点

此时要解决因子乘积最大化的问题，通常，取所有因子的条件概率为高斯分布的形式，则运动数据和观测数据符合：

$$P(x_k|x_{k-1})=N(f(x_{k-1},u_k),R_k)P(z_{kj}|x_k,l_j)=N(h(x_k,l_j),Q_{kj})$$

同样的，它的解法—因子图优化，和之前的类似，也是用GN,LM等。

2.1.3 更具体的因子图

在实际中，我们可能不止有相机，还有其他先验信息—比如，GPS等，它们测到的点是确定的，也就是这些点的先验信息知道了，就可以在图中添加它们的先验信息了，还有编码器，IMU等。如下：

2.1.4 增量的求解方法

无论怎么求，最后都会落到这一步：

$$H\Delta x=g$$

但是，当新的节点和新的边加入，它的所有节点更新量就要重新计算一次更新量。
对资源占用很大。我们继续分析因子图：

• 按照里程计的方式添加节点，在因子图中只有最后一个与之相连的节点会受影响。(实际上是接近的影响大一点)
• 那么我们只需要在有新的变量和因子加入时，分析它和因子图的连接和影响关系：
• • 1. 如果按照回环检测方式添加，回环开始到这一帧这一段的节点都受影响，都要调整
• • 2. 如果只添加一个任意节点，则影响区域几乎只作用于离他最近的点

如下：