OKvis

OKVIS 属于 VIO（Visual Inertial Odometry），视觉融合 IMU 做 odometry。

系统配置

视觉和 IMU 关系图如下： 

待估计的状态量包括机器人在图像  获取时刻的 state  和特征点的坐标  .

其中  包括: 
机器人在世界系下的位置坐标  
旋转矩阵（用四元数表示）  
sensor 系（IMU 系）下机器人的速度 （简写为 ） 
陀螺仪的偏置 ，加速度的偏置 

特征用齐次坐标表示：

相机相对于 IMU 的外参：

相机相对于 IMU 的外参可以在线调整（系统受温度影响机械结构可能产生形变），如果标定的足够准，并且系统运行时参数保持不变，也可以将参数在优化时，设置成常量。

图像 reprojection error 和 IMU error 误差一起优化

OKVIS 是紧耦合的，视觉和 IMU 的误差项和状态量在一起优化。

下图左边表示纯视觉 SLAM 示意图，右边表示加上 IMU 后 SLAM 的示意图。IMU 对相邻两帧的 pose 之间添加约束，而且对每一帧添加了状态量：陀螺仪和加速度计的 bias 和 speed（速度）。

系统优化的能量函数如下，能量函数包括视觉 reprojection error 和 IMU error。

上式中，  表示 frame 索引， 表示 camera 索引， 表示 feature 的索引， 表示特征的 information matrix， 表示 IMU error 的 information matrix。

视觉 reprojection error 形式为：

 
其中  表示相机的投影模型， 表示特征的图像坐标。

reprojection error 从公式中可以看出可以优化系统的 pose ，IMU 和相机的外参 ，和特征坐标 。

IMU 和相机外参的优化项还包括 pose error 和 relative pose error，pose error 是系统初始化时读入的手工标定的相机和 IMU 间的 pose，relative pose 是前后帧相机和 IMU 间的 pose 不能变化太大。

error 还有几项，从代码中也可以看出来，还包括： IMU error，speed and bias error，marginalization error。

**IMU error：**IMU error 代码实现和论文 IMU preintegration on manifold for efficient visual-inertial maximum-a-posteriori estimation 一致，也就是说 OKVIS 优化也是预积分的思路，OKVIS 论文里提到的 IMU 的运动学方程没个毛用。

OKVIS 代码也是按照 preintegration 的思路将前后帧 IMU 测量值做积分，因为积分会用到 IMU 的 bias，而 bias 是状态量，每次迭代时是变化的。所以，每次迭代时，会根据状态量相对于 bias 的雅可比重新计算预积分值，当 bias 变化太大时，不能再用雅可比近似计算预积分值，这时会根据 IMU 测量值重新积分。沈老师的 VINS 系统也是这个思路。

speed and bias error：系统初始化时，会读入手工设定的 IMU 的 bias 参数（包含陀螺仪和加速度计），读入的 bias 作为先验，读入的 bias 和计算的 bias 相减作为 error。speed 没有手工设定，赋值为 0。

marginalization error 和 marginalization 的过程未完，待续。

系统前端过程

Keypoint detection

算法用 SSE-optimized Harris corner detector 做角点检测，用 BRISK descriptor 作为特征描述子。在提取特征的时候尽量使得特征均摊在图像平面上。

在图像特征提取前，先根据系统状态量和 IMU 测量值对当前系统状态做 propagation，根据预测的系统状态（ 和 ）提取特征的描述子。

Keypoint matching

特征匹配有：current frame 和 last keyframes 匹配，current 和 last frames 匹配，和当前帧左右图像间匹配。 
通过 3D-2D 匹配，建立 3D 特征和 2D 特征 reprojection error；通过 2D-2D 匹配初始化新的特征点。

3D-2D 的匹配：通过 IMU 测量值和上一帧系统的状态，预测当前帧系统的状态，首先根据预测的当前帧位姿筛选当前帧可见的特征点（特征点之前初始化获得），然后当前帧的每个特征点和这些可见的特征点做暴力匹配，暴力匹配的方式对于当前帧的每个特征点，计算和当前帧下所有可见的特征点之间特征描述子之间的距离（这里不用类似于 orb-slam 用的 guided matching 策略，作者说因为使用的特征描述子匹配较快，用这种暴力匹配的方式还要快），通过描述子之间的距离确定匹配关系。特征匹配后还进行误匹配的移除，移除分为两步：第一步根据位姿预测的不确定计算 Mahalanobis 距离，判断 3D 到 2D 的匹配是否有效；第二部通过 RANSAC 算法和 OpenGV 库来移除误匹配。

2D-2D 的匹配：匹配也是先通过暴力的方式得到初始匹配，然后通过三角化计算特征的位置，并且移除外点。如果说系统还没初始化，2D-2D 匹配还用于系统的初始化。

Keyframe selection

对于关键帧的选取采取以下策略： 
如果当前帧匹配的特征点在图像中所占的面积不足一定的阈值，或者匹配的特征数目和检测到的特征点数目不足一定阈值，则认为是关键帧。

Keyframes and marginalization

初始阶段：边缘化过程如上图所示，开始的  帧都会被当做关键帧，边缘化的时候会消掉开始几帧 speed/bias 项，如下图所示： 

添加的新的一帧时，当 temporal window 最老的一帧不是 keyframe 时，如上图所示，temporal window 中最老的一帧是  帧，而  帧不是关键帧，这时边缘化掉  帧，边缘化的时候先把  帧所有特征点的观测在优化中全部删除掉（每个观测构成一项重投影误差），然后把  帧的 pose 和 speed/bias 边缘化掉，如下图所示：

temporal window 最老的一帧是 keyframe 时，如上图所示，temporal window 中最老的一帧是 帧，而  帧是关键帧，这时要边缘化掉关键帧  帧，在边缘化的时候会边缘化掉在  中可见而在最新关键帧和当前帧不可见的特征。

下面这幅图也很好的说明了这个问题：

上图中，当 temporal window 中最老的一帧  不是关键帧时，边缘化掉  帧，先把  帧对特征 2,3,4 的观测移除（优化的目标函数中移除），然后边缘化掉  帧的 pose 和 speed/bias。

当 temporal window 中最老的一帧  是关键帧时， 帧能观测到的特征有 1 2 3，当前帧能够观测到的特征有 4，在边缘化  帧时，由于特征 1 在最新关键帧  和当前帧都不可见，所以要被边缘化掉，同时会移除特征 2,3 在  中的观测。