从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)

145
0
2020年10月4日 19时24分

接着上篇:从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)

 

再次说明一下,本博客目前只是我在看深蓝时记录的,当然会有很多 PPT 上的内容,加上一点自己学的时候的理解,或者补全没有写全的推导,目的是能够留下一个电子形式的资料,直接目的是留给自己之后反复查看。每次视频我都是一点点啃下去的,属实对我这种本科生并不友好(本科大牛请绕路),所有内容都是一点点用 LaTeX 打上去的,不是截图搬运。也感谢贺博、高博及深蓝的精美课程!


VIO 残差函数的构建

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图

带权重的残差计算

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(1)
(18)

其中, [公式] 服从高斯分布,协方差为 [公式] 。

  1. 协方差 [公式] 用来 归一化,多传感器融合 [公式] 单位不统一,如:PnP的误差方程的单位是像素,即实际像素点和理论像素点之间的误差;IMU 的误差单位为米( [公式] )和 度( [公式] )。
  2. 权重。协方差越小代表数据越集中, [公式] 就越大,代表权重越高。如说残差方程相差大,协方差大,说明可能存在误匹配 – 坏值,那么 [公式] 较小权重低。

基于滑动窗口的 VIO Bundle Adjustment

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(8)
(19)

高博的视觉SLAM十四讲里,对滑动窗口理论没有做过多的讲述,从上式(19) [公式] 是先验,是后入数据时扔掉的前数据转化成的part, [公式] 和 [公式] 是新窗口内数据的残差方程, [公式] 是上篇提到的鲁棒核函数。

定义优化的系统状态量:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(13)
(20)
  1. [公式] 是 [公式] 时刻 IMU 在惯性坐标系中的位置,姿态,速度,IMU 机体坐标系下加速度和角速度的偏置量估计。 [公式] 是 [公式] 时刻的观测点。
  2. [公式] 分别是滑动窗口里机体状态量和观测路标的起始时刻。N 代表状态量在窗口内的关键帧数,M 是窗口内所有关键帧观测到的路标点数。

视觉重投影误差

一个特征点在 归一化相机坐标系 下的估计值和观测值的差。

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(19)
(21)

其中,待估计的状态量为特征点的三维空间坐标 [公式] ,观测值 [公式] 为特征在相机归一化平面的坐标。

逆深度参数化 Inverse Depth Parametrization

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

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(22)
(22)

其中 [公式] 称为 逆深度[1]。逆深度更容易表示无穷远处的点,当 [公式] ,可以用来表示无穷远处;且 [公式] 更接近正态分布。在实际应用中,逆深度也具有更好的数值稳定性。

 

 

特征点逆深度在第 [公式] 帧中初始化得到,在第 [公式] 帧又被观测到,预测其在第 [公式] 中的坐标为:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(26)
(23)

这里的变换矩阵其实就是先从一帧的相机坐标系 [公式] 这帧的 IMU body 坐标系 [公式] World 坐标系 [公式] 另一帧的 body 坐标系 [公式] 另一帧的相机坐标系。第一帧的 u-v 是可以通过亚像素的方法配准的,故用 [公式] 这一个参数表示第一帧的观测。

视觉重投影误差为:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(27)
(24)

IMU 测量值积分

IMU 的真实值 [公式] ,测量值为 [公式] ,则有:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(30)
(25)
从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(31)
(26)

上标 [公式] 表示 gyroscope, [公式] 表示 Accelerometer, [公式] 表示世界坐标系 world, [公式] 表示imu 机体坐标系 body。

PVQ 对时间的导数可写成:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(36)
(27)
从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(37)

从第 [公式] 时刻的 PVQ 对 IMU 的测量值进行积分得到第 j 时刻的 PVQ:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(38)
(28)

目标→解决:每次 [公式] 优化更新,都需要对之后时间线上的重新积分。

IMU 预积分[2]

积分模型到预积分模型:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(40)
(29)

那么,PVQ 积分公式中的积分项则变成相对于第 [公式] 时刻的姿态,而不是相对于世界坐标系的姿态:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(41)
(30)

预积分量

预积分量仅仅跟 IMU 测量值有关,它将一段时间内的 IMU 数据直接积分起来就得到了 预积分量

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(42)
(31)

PVQ 的积分公式:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(43)
(32)

认为短时间内的 bias 是不变的。

预积分误差:一段时间内 IMU 构建的预计分量做为测量值,对两时刻之间的状态量进行约束。

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(44)
(33)

[公式] 表示只取四元数的虚部 [公式] 组成的三维向量, [公式] 是预积分估计值,四元数 [公式] 的结果是 [公式] 即

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(50)

代表转角 [公式] 接近0,这里只取出 xyz 就是取出旋转角的残差乘以2得到 [公式] 。等号左边每项都是一个三维度的共十五维,除四元数外,其他项均为 j 处世界坐标系下的真值减去 预积分以外的估计值(通过 [公式] 从世界坐标系转换到 i 坐标系),再在 i 坐标系下与预积分值相减得到预积分误差。

预积分的离散形式

这里使用中值法,即两个相邻时刻 k 到 k+1 的位姿用两个时刻的测量值 [公式] 的平均值计算:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(55)
(34)

测量数据 [公式] 都减去了 bias 可以用过标定获得,bias 服从随机游走,故有了后两个式子。随后的目的是 如何求算离散形式的多个 IMU 数据积分形成的累计预积分量的协方差


协方差的传递 Covariance Propagation

已知 [公式] ,则有 [公式] ,证明其实在之前提过:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(58)

假设已知相邻时刻误差的线性传递方程:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(59)
(35)

比如:状态量误差为 [公式] ,测量噪声为 [公式] 。

误差的传递由两部分组成:当前时刻的误差传递给下一时刻,当前时刻测量噪声传递给下一刻。

协方差矩阵可以通过递推得到:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(62)
(36)

其中, [公式] 是测量噪声的协方差矩阵,方差从 i 时刻开始递推, [公式] ;Allan 方差计算出了测量噪声的方差量级。

状态误差线性递推公式的推导

  1. 基于一阶泰勒展开的误差递推方程。
  2. 基于误差随时间变化的递推方程。

基于一阶泰勒展开的误差递推方程

令状态量为 [公式] ,其中,真值为 [公式] ,误差为 [公式] 。另外,输入量 [公式] 的噪声为 [公式] 。

非线性系统 [公式] 的状态误差的线性递推关系如下:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(71)
(37)

其中, [公式] 是状态量 [公式] 对状态量 [公式] 的雅可比矩阵, [公式] 是状态量 [公式] 对输入量 [公式] 的雅可比矩阵。

对非线性状态方程进行一阶泰勒展开有:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(77)
(38)

基于误差随时间变化的递推方程

如果能够推导状态误差随时间变化的导数关系,如:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(78)
(39)

则误差状态的传递方程为:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(79)
(40)

对比两个方法有:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(80)
(41)

为什么会有误差随时间变化这种方法?

因为 VIO 系统中已知状态的导数和状态之间的转移矩阵。比如说速度的导数与状态量之间的关系:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(81)
(42)

那么就可以推导速度的误差和状态误差之间的关系,再每一项上都加上各自的误差就有:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(82)
(43)

预积分的误差递推公式推导

白噪声的值不能像 bias 一样估计,故白噪声不能像 bias 一样减去。

下面误差的 Jacobian 是,在递推公式( [公式] 用了中值法)等式左右分别对偏导分子分母加上一个误差 [公式] 后求出该 Jacobian。对于 [公式] 是用 [公式] 。

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(71)
这个递推公式,可以理解为 k 时刻的该项误差,是由 k-1 时刻的误差及高斯白噪声(不像 bias 可以估计出来减去)递推得出的,但是为了简化,使用了对各项的一阶泰勒展开近似,下面↓来求这里的 F 和 G。
从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(85)

用前面一阶泰勒展开的推导方式,我们希望推导出误差的递推公式:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(86)
(44)

[公式] 为两个时刻间的协方差传递矩阵, [公式] 表示各时刻的误差。 [公式]

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(90)
(45)
从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(91)
(46)

其中系数为:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(92)
(47)

对部分项做详细推导↓对于导数中与变量无关的项省去:

[公式] 对各状态量的雅可比推导, [公式] 第三行

速度预计分量 [公式] 的递推计算形式:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(94)从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(95)
(48)

 

[公式] :对上一时刻速度预积分量的 Jacobian

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(97)
(49)

[公式] :对角度预积分量的 Jacobian

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(99)
(50)
从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(100)
(51)

Part 1 对应的雅可比为:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(101)
(52)

Part 2 对应的雅可比为:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(102)
(53)

合起来就是(47)中的 [公式] 。

[公式] :速度预积分量对 k 时刻角速度 bias 的 Jacobian

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(104)
从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(105)

就有了这样的雅可比推导过程:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(106)
(54)

旋转预计分量的 Jacobian, [公式] 第二行

旋转预积分递推公式:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(107)

[公式] :前一时刻的旋转误差 [公式] 如何影响当前旋转误差 [公式]

假设两个时刻的真值是 [公式] ,不考虑受到角速度 bias 的影响,两时刻的增量

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(112)

三元组四元数相乘有如下性质(其实就是伴随性质,在李群李代数证明过):

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(113)
(56)

其中 [公式] 是和 [公式] 对应的旋转矩阵, [公式] 为 [公式] 的实部, [公式] 为 [公式] 的虚部。

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(119)
(57)
从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(120)
(58)

故第 k+1 时刻的旋转预积分的误差相对于第 k 时刻的 Jacobian 为:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(121)
(59)

残差 Jacobian 的推导

归一化平面视觉残差为:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(122)

对于第 i 帧中的特征点,它投影到第 j 帧相机坐标系下的值为:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(123)

写成三维坐标形式为:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(124)
(60)

这里的 [公式] 是因为是从 [公式] 的变化,是相反的。此处把旋转和平移拆解开,平移部分为, [公式] 的平移部分 [公式] 会经过三次旋转加平移的变换直到 j 的 camera 坐标系下。

变量定义:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(129)
(61)

Jacobian 对状态量,外参和逆深度求导:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(130)
(62)

step 1:先求error对 [公式] 的导数,与 《视觉SLAM十四讲》不同之处是这里在归一化平面。

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(132)
(63)

step 2: [公式] 对 i 时刻的状态量求导。

对位移求导↓

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(133)
(64)

对角度求导↓

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(134)
(65)

只保留与 [公式] 有关的项:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(136)
(66)

这里的 [公式] 即 i 坐标系下的坐标转换到 world 坐标系下。

Jacobian 为:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(138)
(67)

step 3: [公式] 对 j 时刻的状态量求导。

对位移求导↓

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(139)
(68)

负号的原因可以看式(65), [公式] 在式中与 [公式] 相乘,求导固然结果如此。

对角度求导↓

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(142)
(69)

Jacobian 为:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(143)
(70)

step 4:对 imu 与相机之间的外参求导。

就是对两传感器之间的旋转和平移求导。

对位移求导↓

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(144)
(71)

对角度求导↓Jacobian 为:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(145)
(72)

step 5:视觉误差对特征逆深度的求导

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(146)
(73)

IMU 误差相对于优化变量的 Jacobian

预积分误差如下图↓

这里的误差和上部分提到的误差是两码事,我是这么理解的,上部分提到的误差是通过递推 k 到 k+1 时刻间的误差传递;这里的误差是预积分误差,是求出的两时刻的位置、速度与真实值之间的误差。

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(147)

上式中, [公式] 是估计值。

由于 i 时刻的 bias 相关的预积分计算是通过迭代一步一步累计递推的,很复杂。故预积分量直接在 i 时刻的 bias 附近用一阶泰勒展开近似。

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(149)
(74)

对 [公式] 进行推导:

  • 对 i 时刻位移:由于与位移无关,故 Jacobian 为 0;
  • 对 i 时刻旋转:
从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(151)
(75)
  • 对 i 时刻速度: [公式] ;
  • 对 i 时刻的加速度 bias 的 Jacobian,由于加速度只影响预积分,故 [公式] 。

对 [公式] 进行推导:

  • 对 i 时刻姿态求导:
从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(155)
(76)

这里的 [公式] 就是提取 xyz 虚部,乘以系数 2,就是微小转动的 [公式] ,前面有解释。然而(76)式中对分子几个四元数 [公式] ,取共轭后的 [公式] 等于在整个式子前加负号。

  • 对 i 时刻陀螺仪偏置 [公式] :
从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(下篇)插图(159)
(77)

式中对角速度 bias 求 Jacobian,用一阶泰勒展开的形式,最后矩阵内 1 对 [公式] 求导为 0。

 

 

参考

  1. ^Civera J , Davison A J , J. M Martínez Montiel. Inverse depth parametrization for monocular SLAM[J]. IEEE Transactions on Robotics, 2008, 24(5):932-945. http://web.mit.edu/2.166/www/handouts/davison_tro2008.pdf
  2. ^Forster C , Carlone L , Dellaert F , et al. On-Manifold Preintegration for Real-Time Visual–Inertial Odometry[J]. ieee transactions on robotics, 2017, 33(1):1-21. https://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=7557075

发表评论

后才能评论