http://jinjaysnow.github.io/blog/2017-07/ROVIO%E8%A7%A3%E6%9E%90.html
基于EKF的鲁棒视觉惯性测量Robust Visual Inertial Odometry, ROVIO
作者博士论文集:Docteral thesis
滤波结构和状态参数
基于扩展卡尔曼滤波:惯性测量用于滤波器的状态传递过程;视觉信息在滤波器更新阶段使用。
定义三个坐标系:
惯性世界坐标帧:II
IMU固定坐标帧:BB
相机固定坐标帧:VV
对于N个视觉特征点,滤波器状态如下表:
参数 | 坐标系 | 描述 |
---|---|---|
rr | 在IMU固定坐标系BB中 | IMU在IMU坐标系中的坐标 |
vv | 在IMU固定坐标系BB中 | IMU在IMU坐标系中的速度 |
从IMU坐标系BB映射到世界坐标系II | IMU在师姐坐标系中的姿态 | |
bfbf | IMU坐标系BB中 | 加速计偏差 |
bwbw | IMU坐标系BB中 | 陀螺仪偏差 |
cc | IMU坐标系BB中 | IMU-相机外参的平移部分 |
zz | 从IMU坐标BB到相机坐标系VV | IMU-相机外参的旋转部分 |
uiui | 相机坐标系VV中 | 特征点i在V中的bearing向量 |
ρiρi | 无 | 特征i的距离参数 |
上述参数中uiui和ρiρi是将特征点参数化为相对于当前帧的bearing向量和距离参数。这两个参数来自于Montiel发表的逆深度参数化的工作。[TODO]
参数(q,z∈SO(3))(q,z∈SO(3)),单位向量ui∈S2ui∈S2。根据Hertzberg的方法来执行计算差分和微分。[TODO]
滤波器的状态传递过程
记加速计测量值,f~f~,和旋转测量值w~w~。
修正零偏和噪声影响后的IMU加速度和角速度为:
将IMU加速度和角速度转化给相机,乘以旋转外参,使用××表示向量的斜对称矩阵,有:
构造EKF的连续微分方程如下:
其中,NT(u)NT(u)将在bearing向量uu附近将三维向量投影到二维切线空间。具体细节作者还没有公布 still under review...
。[TODO, NT(u)的计算过程在作者博士论文集最后一篇论文中给出NT(u)的计算过程在作者博士论文集最后一篇论文中给出]
滤波器更新过程
对每一帧图像执行一次状态更新。已知相机内部参数,可以计算出bearing向量在图像帧中的像素坐标p=π(u)p=π(u)。对每一个预测会出现在当前图像中的特征点,计算一个二维的线性约束bi(π(u^i))bi(π(u^i))。这个线性约束编码了特征的亮度误差,可以直接作为Kalman更新的innovation。
相应的Jacobian矩阵为:
通过计算图像中存在的特征点的上述约束,可以执行标准的EKF更新。不过,在新特征点初始化还有较大的距离不确定度时,更新过程可能失败。为了避免新初始化的特征点具有较大的不确定性造成的影响,执行了一个基于patch的特征搜索选择合适的特征点。
对于移动的物体或其他干扰,基于Mahalanobis的外点检测用于更新步骤。对比预测的innovation协方差及获得的innovation,然后加权范数查过某一个阈值的测量值。
多层次patch特征处理
预测阶段,多层次patch特征的新位置使用IMU运动模型来计算。特别地,如果外参的标定和特征距离参数收敛,那么会给出特征位置一个高精度的预测。
单个特征的处理流程如图:
Structure and wraping
对于一个给定的图像金字塔(因子为2的下采样)和一个给定的bearing向量uu,多层次的patch通过在每一个图像层次ll相应的像素坐标p=π(u)p=π(u)处提取固定大小(8x8像素)的patch组合得到。跟踪这个特征对于模糊图像具有鲁棒性。
Alignment Equations and QR-decomposition
使用亮度误差来预对齐特征或者更新滤波状态。对于图像IlIl给定的一个图像金字塔和一个给定的多层次patch特征(坐标为pp,patches PlPl), 亮度误差由下式计算:
图像层次ll和patch像素pjpj:
el,j=Pl(pj)−Il(psl+Wpj)−mel,j=Pl(pj)−Il(psl+Wpj)−m
其中尺度sl=0.5lsl=0.5l指代下采样。减去平均的亮度误差mm,可以得到帧间的亮度变化。
应用QR分解来简化计算过程。