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

"议论平恕,无所向背”

  • 1. 位姿图
    • 1.1 具体做法
    • 1.2 小结
  • 2. 因子图
    • 2.1 具体做法
      • 2.1.1 贝叶斯网络
      • 2.1.2 因子图
      • 2.1.3 更具体的因子图
      • 2.1.4 增量的求解方法

引入:

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

办法:

  1. 滑动窗口法:保持H固定大小,丢弃历史
  2. 位姿图:不管路标,只管位姿,从BA变成Pose Graph,容易看出资源会少很多很多,如下:
    【视觉SLAM入门】7.4.后端优化 --- 基于位姿图和基于因子图_第1张图片

1. 位姿图

1.1 具体做法

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

  • 假设,位姿 ξ i \xi_i ξi 经过运动 Δ ξ i j \Delta \xi_{ij} Δξij 变到了 ξ j \xi_j ξj ,根据李群李代数知识,有如下等式:

Δ ξ i j = ξ i − 1 ∘ ξ j = l n ( e x p ( − ξ i ) ^    e x p ( ξ j ^ ) ) ˇ \Delta \xi_{ij} = \xi_i^{-1} \circ \xi_j = ln(exp(-\xi_i)\hat{} \; exp(\xi_j \hat{})) \v{} Δξij=ξi1ξj=ln(exp(ξi)^exp(ξj^))ˇ

用李群表示则如下:

Δ T i j = T i − 1 T j \Delta T_{ij} = T_i^{-1} T_j ΔTij=Ti1Tj

T i j T_{ij} Tij 右移:
在这里插入图片描述
求它关于优化变量 ξ i \xi_i ξi ξ j \xi_j ξj 的导数,按照李群李代数的方法,中间过程略,最终构建的总体目标函数如下:

min ⁡ ξ 1 2 ∑ i , j ∈ ε e i j T Σ i j − 1 e i j \min\limits_{\xi} \frac{1}{2} \sum\limits_{i,j \in \varepsilon} e_{ij}^T \Sigma^{-1}_{ij}e_{ij} ξmin21i,jεeijTΣij1eij

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

1.2 小结

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

2. 因子图

2.1 具体做法

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

2.1.1 贝叶斯网络

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

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

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

P ( x 3 ∣ x 2 , u 3 ) P ( z 1 ∣ x 1 , l 1 ) P(x_3|x_2, u_3) \qquad\quad P(z_1|x_1,l_1) P(x3x2,u3)P(z1x1,l1)

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

{ 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(xkxk1,uk)P(zkxi,lj)


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

2.1.2 因子图

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

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

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

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

P ( x k ∣ x k − 1 ) = N ( f ( x k − 1 , u k ) , R k ) P ( z k j ∣ x k , l j ) = N ( h ( x k , l j ) , Q k j ) P(x_k|x_{k-1}) = N(f(x_{k-1}, u_k), R_k)\qquad P(z_{kj}|x_k, l_j) = N(h(x_k, l_j), Q_{kj}) P(xkxk1)=N(f(xk1,uk),Rk)P(zkjxk,lj)=N(h(xk,lj),Qkj)

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

2.1.3 更具体的因子图

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

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

2.1.4 增量的求解方法

无论怎么求,最后都会落到这一步:

H Δ x = g H \Delta x = g HΔx=g

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

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

如下:
【视觉SLAM入门】7.4.后端优化 --- 基于位姿图和基于因子图_第5张图片

你可能感兴趣的:(自动驾驶-SLAM,#,VSLAM,SLAM,优化,计算机视觉,自动驾驶,linux)