0. 内容
1. VIO回顾
整个视觉前端pipeline回顾:
- 两帧图像,可提取特征点,特征匹配(描述子暴力匹配或者光流)
- 已知特征点匹配关系,利用几何约束计算relative pose([R|t]),translation只有方向,没有尺度
- 使用三角化获得3维坐标,即可完成vslam系统的初始化
- 有了3D特征点,后续可根据特征跟踪,使用PnP求解Camera Pose,无需再使用几何约束
-
IMU的加速度要和世界系的重力进行对齐
世界系重力假设(0,0,-9.81),IMU第一帧估计出一个 c g = c R c b g b ^{c}g=^{c}R_{cb}g_b cg=cRcbgb,就可以求出第一帧camera在world下的pose(带头大哥) -
视觉的尺度和IMU尺度要进行对齐
-
VIO系统速度,传感器bias需要估计
-
IMU和相机的外参Tic
知道了z轴的法向量就能估计roll和pitch(tilt),但是yaw不可观。
2. VINS鲁棒初始化
2.1 pipeline overview
IMU积分是米制单位,但是camera不是,camera存在一个缩放因子,标定出外参 [ R b c , t b c ] [R_{bc},t_{bc}] [Rbc,tbc]之后,可以求得尺度因子s,先对坐标系,R,t的数学表示做一下说明:
旋转脚标相连时相消,刚性连接在不同时刻的的观测相同,平移左上角是观测系,右下角从左到右;平移乘旋转后平移向量不变,但观测系改变。
式(4)第一行这样理解:
c 0 p ‾ c 0 b k = c 0 p ‾ c 0 c k − 1 s c 0 R c 0 b k b p b c ( 2.1 ) {^{c_0}\overline p_{c_0b_k}}={^{c_0}\overline p_{c_0c_k}}-\frac{1}{s}{^{c_0} R_{c_0b_k}} {^{b}p_{bc}} (2.1) c0pc0bk=c0pc0ck−s1c0Rc0bkbpbc(2.1)
左上角都是观测系,即在什么系下看这个量,由于刚性连接在不同观测系,不同时间下相同,且平移乘旋转后旋转向量不变,但观测系改变,所以 b p b c = b k p b k c k ( 2.2 ) {^{b}p_{bc}}={^{b_k}p_{b_kc_k}}(2.2) bpbc=bkpbkck(2.2)
所以将(2.2)带入式(2.1)可得:
c 0 p ‾ c 0 b k = c 0 p ‾ c 0 c k − c 0 p ‾ b k c k = c 0 p ‾ c 0 c k + c 0 p ‾ c k b k ( 2.3 ) {^{c_0}\overline p_{c_0b_k}}={^{c_0}\overline p_{c_0c_k}}-{^{c_0}\overline p_{b_kc_k}}={^{c_0}\overline p_{c_0c_k}}+{^{c_0}\overline p_{c_kb_k}}(2.3) c0pc0bk=c0pc0ck−c0pbkck=c0pc0ck+c0pckbk(2.3)
即在 c 0 c_0 c0系下进行将 c 0 p ‾ c 0 c k + c 0 p ‾ c k b k {^{c_0}\overline p_{c_0c_k}}+{^{c_0}\overline p_{c_kb_k}} c0pc0ck+c0pckbk向量相加,不难理解。
标定出Camera和IMU外参之后,利用式(2.1)就可以求出尺度因子。
Tbc外参的Rotation很重要,而Translation由于可能离得比较近,所以相对来说没有Rotation重要
2.2 外参估计
旋转约束:两种路径求取的 q c k b k + 1 q_{c_kb_{k+1}} qckbk+1应该相同。
这里的未知量只有外参 q b c q_{bc} qbc,也可以简单地理解为一段时间内,camera出一段pose(trajectory),imu积分出一段pose(trajectory),这两段pose理想情况下只有外参的差异,(不考虑 t b c t_{bc} tbc的情况下)可以将这两段pose align起来,残差是两段pose align的残差,待估计量就是外参,做LSP的结果就是外参。
VINS论文中将多个时刻的数据累计起来,并使用了鲁棒核函数对每一项进行了加权,权值计算方法:有 t r ( R ) = 1 + 2 c o s θ tr(R)=1+2cos\theta tr(R)=1+2cosθ(可以看wiki),式(9)中的那一大块矩阵就是式(5)移项而得,因为求的是相同的量(如 q c k b k + 1 q_{c_kb_{k+1}} qckbk+1),所以理想情况下连乘应该是Identity,但实际上,如果外参估的不准,这个角度会较大(>threshole),所以式(8)将大于threshold的部分的权值设的较小,进行了抑制。
最终对 q b c q_{bc} qbc的求解还是对式(7)使用SVD分解,取 V T V^T VT最后一列作为解。
(参考这篇博客参考这篇博客中使用Lanrange算子证明取 V T V^T VT最后一列是我们所求的解。)
论文中还对SVD分解的倒数第二小奇异值进行了判断,如果小于阈值则认为该次估计无效,重新采集数据进行估计。(这和增多数据来提升数据信噪比是一样的目的,都是为了提高数值稳定性)
2.3 gyro bias估计
式(10)就是将 b k b_k bk到 b k + 1 b_{k+1} bk+1时刻之间imu的rotation都转到 c 0 c_0 c0系下进行align:
- 前两项四元数相乘代表求在 c 0 c_0 c0系下看imu在 b k b_k bk到 b k + 1 b_{k+1} bk+1时刻之间的relative rotation
- 第3项 q b k b k + 1 q_{b_kb_{k+1}} qbkbk+1是IMU积分而来的两时刻间的relative rotation真值(会受到gyro bias的影响),对其进行一阶Taylor展开,带入(10),(10)即代表一阶项所代表的 θ e r r \theta_{err} θerr,可以求出gyro bias
这跟前面估计外参的是一样的,都是使用的旋转约束。