【SLAM】VINS-MONO解析——后端优化(理论部分)

141
0
2020年10月28日 09时04分

7.后端非线性优化

7.1 理论基础

7.1.1 bayes模型,因子图和最小二乘
这一部分主要是对董靖博士在公开课《因子图的理论基础》上的回忆和总结。
(1)bayes模型
假设有黄色是机器人在不同时刻的位姿,蓝色是机器人观测到的路标点,红色是机器人对路标点的观测,绿色是机器人对自身运动的观测。

 

1

 

那么,这个过程可以用下面这个bayes-net来描述:

 

2

 

那么,可以有这样的定义:
黄色——机器人位姿——是状态量——X;
蓝色——路标点坐标——是状态量——X;
红色——机器人对路标点的观测——是观测量——Z;
绿色——机器人对自身运动的观测——是观测量——Z;

状态量X,对应着 待优化量,也就是g20里的vertex,ceres里的ParameterBlock;
观测量Z,对应着残差/因子,也就是g20里的edge,ceres里的ResidualBlock;

那么根据bayes法则,观测量Z和状态量X联合概率等于条件概率乘以边缘概率:

 

3

 

P(Z | X)是观测量Z对应的概率,也叫似然;
P(X, Z)是观测量Z和状态量X的联合概率;
P(X)是状态量的先验概率,比如说世界坐标系,这就是一个先验,假如说没有世界坐标系这个先验信息,那么系统整体性的漂移都能满足求解要求,那么就多了一个自由度的不可观。
而观测变量Z有一个特点,例如左一红圈,只跟状态变量x1和l1有关,而与路标点l2无关。其它的观测变量也有类似的特点,也就是稀疏性和不相关性这两个特点,所以整体的观测P(Z | X)又可以以每一个单独的观测叠乘来获得。

 

4

 

(2)因子图
首先要明确我们的求解目标是P(X | Z)最大时,也就是在观测Z的情况下,对应的X;
因为往往状态量X是不知道的,而在状态X下的观测Z,也就是P(Z | X)是知道的;
所以,需要找到待求量P(X | Z)和已知量P(Z | X)之间的关系:

 

5

 

P(X | Z)等于上式中中间的式子,正比于右边的式子。
P(Z)是观测的先验信息,对应的是测量传感器的数学模型,例如相机,IMU,轮速器。

我们想找到这么一组状态量X,使得在当前观测Z的情况下,出现状态P(X | Z)的概率最大!
可以转换为求解,在当前状态下对应的观测P(Z | X)乘以一个状态量的先验P(X)!

 

6

 

这就是传说中的,最大后验估计,转化为求似然乘以状态量的先验!

(3)因子的定义

 

7

 

那么,

 

8

 

就可以转换为,

 

9

 

因子的定义如下:

 

10

 

里面的f(xi)就是观测量的残差!也就是我们常说的IMU残差,重投影误差,先验误差等。
之所以用e的指数函数,因为传感器的测量噪声一般都满足gauss分布。

当因子连乘后,对应的最大似然估计就是最小二乘。

 

11

 

(3)残差,状态量,J,H的对应位置关系
因子图它最方便的地方就是能一眼确定这是什么类型的残差,还能知道当前残差对哪个状态量X的J块是有值的,哪块是0的!

 

12

 

就比如说左一红色方框因子,我们能一眼知道是套用重投影误差模型,对应的J是什么公式早就有了;它连着x1和l1,说明只有这两个位置的J是有值的。
不过需要注意的是,vins的因子图一般是下面这种形式的。同一个特征点是被2帧观测到的(重投影),而某一个IMU因子与相邻两帧的PVQ和bias相连的。

 

13

 

因此,往J大矩阵里面塞每一行非常方便!这也是g2o的逻辑,加状态量叫做加入vertex,加入一个残差,也就是J矩阵的一行,叫做加edge!
考虑到pose的数量远小于特征点的数量,在shur操作的时候一般会求pose部分的状态量,所以一般会把pose相关的edge放在J矩阵的上方。

7.1.2 后端模型
首先确定状态量X,

 

14

 

包括各帧的PVQ,bias,外参,特征点的逆深度。状态量的数量决定了H矩阵的维度和J矩阵的列数。PQ这两类状态是视觉和IMU都会优化的量。
目标函数为:

 

15

 

这三项分别为边缘化的先验信息、IMU的测量残差、视觉的重投影误差。这个目标函数就解释了vins系统为什么是紧耦合的系统,因为三个目标函数放在一起优化的!
对于这样的目标函数,可以构造一个下式所示的增量公式,迭代求解状态量X的最优解:

 

16

 

根据上一节内容,我们知道Jacobian和H是可以一行一行(若干行若干行)往里面加,接下来我们研究一下,怎么样往里面加。

 

17

 

上面这个因子图非常经典地讲解了IMU约束和视觉约束。

(1)IMU约束部分
在4.1.1.IMU预积分部分,我们已经知道:

 

18

 

等号左边减去右边,就是IMU残差:

 

19

 

优化变量是相邻两帧 (第bk、bk+1时刻)p、q、v、ba、bg:

 

20

 

这一部分,残差是15维的,状态量是7+9+7+9维的,所以说,每增加一个IMU约束,总的Jacobian矩阵增加了15行,增加了7+9+7+9列!(需要注意的是,四元数是4维的,但是Localdemension是3维的)。

接下来,我们需要弄清楚,残差对状态量的Jacobian是什么,对应位置补充上这个J的矩阵块就行了,其他位置还是0。崔神的推到结果如下。
残差对bk时刻的PVQ的Jacobian:15*7

 

21

 

残差对bk时刻的的速度/bias的Jacobian:15*9

 

22

 

残差对bk+1时刻的PVQ的Jacobian:15*7

 

23

 

残差对bk+1时刻的的速度/bias的Jacobian:15*9

 

24

 

然后就是在构造对应的H矩阵块的时候,需要乘上信息矩阵,JT (Pk+1) J + uI,也就是4.2对应部分。

 

26

 

(2)视觉约束部分
这部分的残差就是重投影误差,注意是在归一化平面上表示的:

 

27

 

但是和纯视觉不同的地方是,待优化的状态量X中的旋转平移,都是IMU系到w系到,而不是camera系到w系的:

 

28

 

这里的状态量有两个特点:
待优化量和IMU约束部分的待优化量有重叠,再次体现了紧耦合;
和IMU不同的是,IMU每次优化的状态量是相邻两帧的,但是视觉优化的2帧不一定是相邻的,因此用i,j表示。

特征点在归一化相机坐标系与在相机坐标系下的坐标关系为:

 

29

 

这块没有采用深度,而采用逆深度,因为逆深度满足高斯分布。

对于第 l 个路标点 P,将 P 从第一次观看到它的第 i 个相机坐标系,转换到当前的第 j 个相机坐标系下的像素坐标:

 

31

 

这个转换过程,经历了从ci坐标系-> bi坐标系->w坐标系-> bj坐标系-> cj坐标系这个过程。
上面这个等式,也就是:

 

32

 

那么,在归一化平面上的重投影坐标就是fcj = [xcj/zcj, ycj/zcj],重投影误差就是开头的公式。

这一部分,残差是2维的,状态量是7+7+m维的,所以说,每增加一个视觉约束,总的Jacobian矩阵增加了2行,增加了7+7+m列!(需要注意的是,四元数是4维的,但是Localdemension是3维的)。

因为根据链式法则,对Jacobian的计算可以分成:
1)视觉残差对重投影3D点fcj求导。

 

33

 

2) 再乘上fcj对各个优化变量求导。

 

34

 

然后就是在构造对应的H矩阵块的时候,需要乘上信息矩阵,JT (Pk+1) J + uI,视觉约束的协方差与标定相机内参时的重投影误差有关。VINS在代码中sqrt_info取1.5个像素,对应到归一化相机平面还需除以焦距f,最后得到的信息矩阵:(这里真正的信息矩阵其实是sqrt_info的平方)

 

35

 

(3)先验约束部分
放在8.1节介绍。

发表评论

后才能评论