VIO算法总结（一）

所谓VIO（visual inertial odometry）就是视觉传感器（camera）+IMU（inertial measurement unit）一起来做自主定位。目前主要应用手机AR，无人车高精地图，无人机等领域。这个技术目前做的怎么样呢？理论很成熟，但是纯靠vio去解决定位的应用并不多。手机AR算是一个比较成功单靠vio做的比较好应用，automated driving， UAV 和mobile robot 基本没有全靠vio来做的。

我把VIO分成前端和后端两个部分。顺便总结一下知识点。

一：视觉和IMU前端数据处理

对于视觉前端做法基本分为三种：

①特征点提取（feature detection）+根据描述子匹配特征点

主流的算法如orbslam， okvis都这采取的这个算法，优点是因为有描述子，因此对地图的维护很方便（包括重定位，闭环，全局优化）。这点很重要，尤其是对室内环境，当视觉上共视关系较多的时候，这种方法能很大提高定位精度和局部的稳定性。比如手机AR，室内移动机器人建议使用这种方法。缺点是每帧图像都要提取特征点数量的描述子，有些浪费时间（其实提取描述子时间没有大多数人想象的那么多时间，1000特征点，在3000元的手机上提取brief描述子的话，大概在10ms左右。）还有个缺点是在tracking过程中，如果出现运动过快（图像出现模糊）比较容易tracking failed。而光流要好一些（其实这个差距不是特别明显）。

②特征点提取+光流跟踪

主流的比如vins，svo的初始化（后面的方法也类似）。优点简单高效，tracking要鲁棒一些。缺点很明显：不容易构建全局map，视觉约束只靠sliding window里面的关键帧。比如vins的闭环和重定位是需要另外在提取特征点和描述子的。而且klt的tracking时间也很长（大概需要20ms，在移动手机上）。那vins做的为啥那么好呢？因为它后端做的好，这个后面再讲。rovio方法类似这种。

③直接法

主流方法有lsdslam，dso。优点在弱纹理下，鲁棒性好。缺点明显，不容易维护全局地图，对光照影响较大，高精度地图中无法使用等。

视觉前端涉及的知识点。

初始化的2D-2D的对极几何。

fundamental matrix，essential matrix ，homography matrix和SVD

fundamental matrix = k.inverse().transpose()*essential matrix*k.inverse(),其中k是相机的内参。

p1*E*p2=0 这是基本矩阵的约束，其中p1,p2是匹配点的归一化坐标即（u1,v1,1)和(u2,v2,1).E是essential matrix。这里可以看出每对匹配点会产生一个约束，由于E=t^ * R 其中t^是translation的斜对称矩阵，因此E共有6个自由度，又因为尺度的原因，E只有5个自由度，所以至少需要5个匹配点。

homegraphy 认为两张图像上的点都在一个平面上，因此有：p2=H*p1。H有8个自由度，因为每个匹配点可产生2个约束，因此求解它至少需要4对matchs。

在slam中会有很多线性方程求解的地方，而SVD分解是最好用的工具。假设有A*x =0的形式, 假设A是一个m×n的矩阵，如果A是满秩的方正，那么x有唯一解。如果mn则无解，我们可以根据奇异值分解出的最小奇异值对应的右奇异特征向量求解。

对于IMU前端

IMU前端基本都是使用预积分的方式，和积分的区别（预积分把gravity考虑进去了）。imu预积分主要是在两帧图像之间跟新imu的15个状态变量（p, v, q, ba, bg）的Jacob和covariance。

假设我们要积分的非线性微分方程为：

x' = f(t,x)

积分的时间间隔为delta_t,the euler method assumes that f(t,x) is constant over delta_t,则：

x(t+delta_t) = x(t) + delta_t * f(t,x)

但是在delta_t时间段内 f(t,x)并不是常量，因此一种closed-form integration methods 出现了，假设

x'(t) = A* x(t), 注意这是连续时间上的关系。

假设积分的时间间隔是（t，t+delta_t),则：

Xn+1 = exp(A*delta_t) * Xn   =>    exp(A*delta_t) = I + A*delta_t + 1/2 * A*A * delta_t*delta_t + ...

如果取泰勒一阶近似，我们就可以更新离散时间下的jacob和covariance：

jacobian =  （I + A*delta_t） * jacobian;
covariance =  （I + A*delta_t） * covariance * （ I + A*delta_t） + V * noise * V.transpose()

每次预积分的初始值：jacobian是单位矩阵，covariance是零矩阵。noise是白噪声大小，V是噪声的状态转移矩阵。

为啥要求jacobian和covariance？jacobian主要是为了求一阶近似的p， v，R

Eigen::Quaterniond corrected_delta_q = delta_q * Utility::deltaQ(dq_dbg * dbg);
Eigen::Vector3d corrected_delta_v = delta_v + dv_dba * dba + dv_dbg * dbg;
Eigen::Vector3d corrected_delta_p = delta_p + dp_dba * dba + dp_dbg * dbg;

covariance主要是为了求imu误差项的权重：

Eigen::Matrix sqrt_info = Eigen::LLT>(pre_integration->covariance.inverse()).matrixL().transpose();

IMU积分还有个问题就是初值的确定（initialization），尤其velocity，ba，bg。至于初始的position 和 quaternion可以假设为0，和单位矩阵。这个初值一般结合visual information 来求，比如vins-mono。