rovio论文解读，及工程应用经验总结

rovio标签：基于滤波(EKF)，紧耦合（fully integrate visual features into the state of the Kalman filter）

优点：不必初始化，计算量小（稀疏图像块EKF）

源码及安装使用（参考官网）

三篇原作论文

论文4个创新点

论文组织架构

Ⅱ filter setup，EKF滤波器建模：关键是状态空间的定义（加入了图像特征），状态更新过程

Ⅲ multilevel patch feature handling, 多级图像块特征处理策略

Ⅳ 实验

论文中的关键术语解释

rovio算法的假定前提条件

rovio工程应用要考虑的要点

1、传感器选型

2、时间同步

3、算法调参

一些思考

1、提取特征时，为何不直接用像素，而是图像块？

2、如何约束EKF状态空间的规模到一个合理范围，且维护稳定的状态（特征）？

rovio算法框架流程图

暂未搞清楚的地方

自己动手跑起来

源码及安装使用（参考官网）

https://github.com/ethz-asl/rovio

作者自己开发了一个EKF滤波库lightweight_filtering，rovio中的EKF通过调用该库实现，因此rovio编译安装依赖于此库。

三篇原作论文

http://dx.doi.org/10.3929/ethz-a-010566547 (IROS 2015)
http://dx.doi.org/10.1177/0278364917728574 (IJRR 2017)

作者博士大论文：《State Estimation for Legged Robots - Kinematics, Inertial Sensing, and Computer Vision》

论文4个创新点

以下分析论文http://dx.doi.org/10.3929/ethz-a-010566547 (IROS 2015)

首先，论文提出了VO中的几个核心问题，引出其创新点：

（1）consistency一致性、平滑度，此问题由VO数学建模固有非线性导致（inherent nonlinearity）。

解决方案2个：一是跟踪的特征点表示在本体坐标系，二是在计算雅可比时使用特定线性化点

（2）路标点特征表示方法：传统方法使用空间位置坐标，存在问题是初始化时尺度信息非常不确定，对后续估计跟踪不利影响。替代方法是采用bearing vector表示特征点。

4个创新点：针对每个创新点同时思考两个问题，why？作者为何这样做（有啥优点）？ how？作者如何实现的？

（1）使用bearing vector和distance parameter来参数化表示点特征（降低了路标点表示维度有助于提升计算速度，提升连续性？与创新点4相呼应，bearing vector通常表示在相机坐标系下）

（2）EKF直接同时跟踪多个多级图像块特征，块亮度误差作为EKF的测量残差项

（3）使用QR分解降低最小二乘问题规模，加快EKF求解速度

（4）包含完全状态的EKF，该EKF为基于载体视角（估计的路标点位置属于本体坐标系，因此不存在累积误差）

论文组织架构

Ⅱ filter setup，EKF滤波器建模：关键是状态空间的定义（加入了图像特征），状态更新过程

A. EKF滤波器结构，状态空间定义

坐标系系统：惯性导航系，IMU本体系，相机系。

状态空间：r位置，q姿态，v线速度，u图像特征表示为bearing vector（一帧图像共跟踪N个特征，实验部分说同时跟踪50个特征下定位频率达20HZ）。其他：b加速度计陀螺仪偏差，c/z相机IMU外参，ρ特征距离

B. 状态预测

IMU-driven，由imu数据驱动的EKF状态预测

C. 状态更新

使用图像块亮度误差作为EKF的测量残差项
为了提高跟踪鲁棒性，针对运动物体或其他干扰，rovio采用基于Mahalanobis检测outliers并从状态空间剔除这些干扰特征

Ⅲ multilevel patch feature handling, 多级图像块特征处理策略

A. structure & warping

通过跟踪每个图像块内2个方位向量来计算affine warping matrix ？

B. alignment公式，QR分解

最小二乘问题建模

论文计算图像intensity errors来pre-align特征或来更新EKF状态，那么如何构建最小二乘问题呢？

通过计算各级金字塔图像I（输入）与多级图像块特征（已知）之间的亮度误差，即最小化亮度误差和，该误差作为驱动EKF状态更新的残差项。

C. 特征提取，特征筛选

特征的提取

使用何种特征？角点特征。
使用何种特征提取算法？采用一种快速角点检测算法来提取角点特征
如何选择有效特征？还是打分机制，采用adapted Shi-Tomasi score打分，好处（1）分数高低直接反映了alignment精度（2）允许计算1或2范数特征值，从而在角点特征稀少场景中能够转换为提取边线特征。

知识加油站：Shi-Tomasi score

我们知道Harris角点检测的打分公式为： R = λ1 λ2 - k( λ1 + λ2)²

但是Shi-Tomasi使用的打分函数为： R = min(λ1， λ2)

如果打分超过阈值，我们就认为它是一个角点。我们可以把它绘制到 λ1~ λ2空间中，就会得到下图：只有当 λ1和 λ2都大于最小值时，才被认为是角点（绿色区域）。比Harris角点检测的效果好

https://blog.csdn.net/qq_36387683/article/details/80550964

不稳定特征的剔除

为何要做特征筛选（剔除）？一帧图像提取的角点特征数量可能会非常巨大，不可能全部加入到EKF状态空间中进行跟踪。因此，需要对原始提取的特征进行筛选，只保留最稳定的那些特征。

如何筛选稳定特征，剔除不稳定特征？依然是打分机制，具体操作为计算特征跟踪分数，代表了被连续跟踪的情况，包含局部跟踪分数（最近若干帧）、全局跟踪分数（从最初被检测开始到当前帧）。这里需要设置一个分数阈值，可以是固定阈值，也可以为自适应阈值（论文采用）。

Ⅳ 实验

包括：实验设置、低速运动实验、高速运动实验、UAV飞行实验

实验设备：相机和IMU。使用双目相机，集成了IMU，且相机与IMU时间是硬同步的hardware time-synchronized，因此，论文实验效果很好。这也

性能：能够运行在UAV上，速率20HZ（每帧跟踪50个特征）

改进：（1）针对发散问题的处理策略（2）支持多相机，带来的好处为针对缺少平移运动的情况能够提高滤波器效果（3）在线标定支持估计相机内参

论文中的关键术语解释

patch：定义一个固定大小正方形图像块（例如8*8pixel）

patch feature：rovio使用固定尺寸正方形图像块特征，而非单个像素特征

multilevel patch feature：针对输入的多级金字塔图像对应的块特征，即每一级金字塔图像对应一级块特征

bearing vector： $\mu$ ，使用图像特征被以bearing vector形式加入到状态空间中

参考 OpenGV--A library for solving calibrated central and non-central geometric vision problems

https://laurentkneip.github.io/opengv/page_how_to_use.html

方位向量：用2D（方位角，仰角）来表示3D路标位置，降低了信息维度，定义在相机坐标系。下图红色向量即为方位向量。

patch coordinate： $p=\pi (\mu )$

patch alignment：块特征对齐，前后两帧图像中块特征的匹配，估计变换矩阵实现对齐

affine warping matrix：仿射变形矩阵（保持平行性）

patch intensity gradient：块亮度梯度

warp path：图像块仿射变换？

rovio算法的假定前提条件

任何算法都不是万能的，而是只能在有限特定环境（条件）下工作的，即算法数学建模需要满足一些假设条件。

假设1、各个特征之间的距离>图像块尺寸

假设2、图像块中所有像素点的亮度噪声值相同。

assume that the additive noise magnitude on the intensities is equal for every patch pixel（additive discrete Gaussian pixel intensity noise），这里的noise是相机模型观测噪声为离散高斯模型，代表每个像素的亮度噪声

rovio工程应用要考虑的要点

1、传感器选型

相机与IMU，尤其是IMU的性能直接影响位姿估计跟踪效果。建议用好一些IMU

2、时间同步

时间同步误差大将大大降低跟踪精度，建议采用支持硬时间同步的相机+IMU，或至少采用global-shutter相机，可选方案推荐：

（1）论文使用的装备

synchronized global-shutter camera (Aptina MT9-V034 at 20 Hz) and IMU (Analog Devices ADIS16488 at 200 Hz).

（2）网友开发的装备：使用一个全局快门相机、一个低端IMU，通过arduino实现时间硬同步，将同步时间戳发布出来了。以下给出几个重要参考网址：

软硬件环境、相机内参标定、相机IMU外参联合标定：http://grauonline.de/wordpress/?page_id=2014

bluefox2相机与IMU同步配置：http://grauonline.de/wordpress/?page_id=1951

Matrix-Vision Bluefox usb2.0 MLC cameras (bluefox2)全局快门相机ROS驱动：https://github.com/KumarRobotics/bluefox2卷帘快门（Rolling Shutter）与全局快门（Global Shutter，通过整幅场景在同一时间曝光实现的。Sensor所有像素点同时收集光线，同时曝光。）

https://blog.csdn.net/abcwoabcwo/article/details/93099982

3、算法调参

算法参数对rovio跟踪效果影响巨大，针对不同传感器与特定场景都需要重新调参

一些思考

1、提取特征时，为何不直接用像素，而是图像块？

论文Ⅲ.A部分及Ⅱ.C部分给出答案：好处如下：（1）首先，必须满足的条件是图像块允许直接计算亮度误差（反馈给EKF更新）（2）对于初始估计不准确及图像模糊情况具有较高鲁棒性（3）与传统重投影误差算法（需要根据相机内外参投影到图像平面计算像素误差）相比，图像块（不必投影，直接计算亮度误差）考虑了纹理特征，因此误差模型精度更高

2、如何约束EKF状态空间的规模到一个合理范围，且维护稳定的状态（特征）？

将图像块特征加入到状态空间中并使用EKF进行跟踪，从而构建了基于滤波的紧耦合VIO，但产生一个核心问题是：图像特征点太多了，如果全部加入到EKF状态空间，那么计算量将非常大，导致无法实时跟踪。那么如何解决呢？针对此问题的解决方案是rovio论文的一个创新点，做法其实挺直白：

（1）采用QR分解降低状态空间维度（降维打击），即减小了求解最小二乘问题的线性方程组的规模。

（2）对已存在状态空间中的图像块特征进行打分，高者留，低着走。具体方法很简单即通过统计该图像块连续n帧跟踪情况来打分。这种打分机制是动态更新的（非终身制），因此不会出现不定的特征还会占着茅坑不拉屎，这机制跟粒子滤波的粒子权重重采样相似（动态更新粒子权重，保留好粒子，剔除不良粒子）。

补充一下，上面说的计算量很大，具体是指什么计算量呢？

rovio通过计算图像块intensity errors即图像块像素亮度值误差构建最小二乘问题，通过高斯牛顿来求解这个最小二乘问题，图像块特征数量决定了线性方程组系数矩阵维度，因此，如果把所有特征都加进来进行处理，那矩阵维度必然超大，求解速度会很慢，所以计算量就是求解这个最小二乘问题的计算量。

rovio算法框架流程图

针对图像块包含以下3个核心操作：先提取块，EKF预测，再对图像块仿射变形，计算图像块亮度平方和最小误差作为EKF更新步骤中的残差

extract path：提取图像块特征（角点）

warp path：仿射变形？

align path：图像块匹配

暂未搞清楚的地方

主要是Ⅲ.A部分

1、affine warping

2、feature distance

自己动手跑起来

场景：室内，单目+imu安装在四足机器狗上，用于估计机器人运动定位及构建高程地形图。

设备：bluefox2全局快门单目，xsens IMU，apritag标定板。使用了多种单目测试，单目与IMU安装也尝试了多种方式

ROS节点：VLP16三维激光雷达、单目相机、rovio、em建图、tm建图

运行效果：里程计定位输出