ROVIO解析

http://jinjaysnow.github.io/blog/2017-07/ROVIO%E8%A7%A3%E6%9E%90.html

基于EKF的鲁棒视觉惯性测量Robust Visual Inertial Odometry, ROVIO

作者：Michael Bloesch

作者博士论文集：Docteral thesis

滤波结构和状态参数

基于扩展卡尔曼滤波:惯性测量用于滤波器的状态传递过程；视觉信息在滤波器更新阶段使用。

定义三个坐标系：
惯性世界坐标帧：II
IMU固定坐标帧：BB
相机固定坐标帧：VV

对于N个视觉特征点，滤波器状态如下表：

参数	坐标系	描述
rr	在IMU固定坐标系BB中	IMU在IMU坐标系中的坐标
vv	在IMU固定坐标系BB中	IMU在IMU坐标系中的速度
qq	从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加速度和角速度为：

f^=f~−bf−ωfω^=ω~−bω−ωωf^=f~−bf−ωfω^=ω~−bω−ωω

将IMU加速度和角速度转化给相机，乘以旋转外参，使用××表示向量的斜对称矩阵，有：

v^Vω^V=z(v+ω^×c)=z(ω^)v^V=z(v+ω^×c)ω^V=z(ω^)

构造EKF的连续微分方程如下：

r˙v˙q˙b˙fb˙ωc˙z˙u^iρ^i=−ω^×r+v+ωr;=−ω^×v+f^+q−1(g);=−q(ω^);=ωbf;=ωbw;=ωc;=ωz;=NT(ui)ω^V−[0−110]NT(ui)v^Vd(ρi)+ωu,i;=−uTiv^V/d′(ρi)+ωρ,i;位置的导数=速度=角速度×当前位置+当前速度+速度+位置白噪声速度的导数=加速度=角速度×当前速度+加速度测量值−重力加速度姿态=当前姿态⋅角速度⋅δt,求导有：姿态的导数=当前姿态⋅角速度加速计零偏的导数为0，需要加上白噪声的影响，故而加速计零偏导数=白噪声陀螺仪零偏的导数为0，需要加上白噪声的影响，故而陀螺仪零偏导数=白噪声IMU与相机的平移外参导数为0，加上白噪声的影响，故而平移外参导数=白噪声IMU与相机的旋转外参导数为0，加上白噪声的影响，故而旋转外参导数=白噪声r˙=−ω^×r+v+ωr;位置的导数=速度=角速度×当前位置+当前速度+速度+位置白噪声v˙=−ω^×v+f^+q−1(g);速度的导数=加速度=角速度×当前速度+加速度测量值−重力加速度q˙=−q(ω^);姿态=当前姿态⋅角速度⋅δt,求导有：姿态的导数=当前姿态⋅角速度b˙f=ωbf;加速计零偏的导数为0，需要加上白噪声的影响，故而加速计零偏导数=白噪声b˙ω=ωbw;陀螺仪零偏的导数为0，需要加上白噪声的影响，故而陀螺仪零偏导数=白噪声c˙=ωc;IMU与相机的平移外参导数为0，加上白噪声的影响，故而平移外参导数=白噪声z˙=ωz;IMU与相机的旋转外参导数为0，加上白噪声的影响，故而旋转外参导数=白噪声u^i=NT(ui)ω^V−[01−10]NT(ui)v^Vd(ρi)+ωu,i;ρ^i=−uiTv^V/d′(ρi)+ωρ,i;

其中，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。

yi=bi(π(u^i))+niyi=bi(π(u^i))+ni

相应的Jacobian矩阵为：

Hi=Ai(π(u^i))dπdu(u^i)Hi=Ai(π(u^i))dπdu(u^i)

通过计算图像中存在的特征点的上述约束，可以执行标准的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分解来简化计算过程。