论文翻译Trifo-VIO: Robust and Efficient Stereo Visual Inertial Odometry using Points and Lines

Some ideas

视觉SLAM发展至今，理论框架与技术框架都已经逐渐成熟与定型。新的工作仍然是在原有工作基础上的组合与优化，也包括本篇文章。然而，目前的SLAM框架在真实的场景应用中仍然存在切实的挑战，这是学术界有目共睹的事实。唯物主义和唯心主义共同塑造了我们的信念，我们仍然抱有希望，下一代的SLAM框架将更加优秀和智能，其所面临的问题将在更高的维度。但是我们仍然坚信，新事物是从旧事物中发展而来，对旧事物的深刻理解，是破坏旧事物的前提。尽管我们不一定选择了正确的方向，但是我们希望选择一种正确的方式。
视觉惯性里程计是视觉里程计领域中目前最有潜力的方向之一，其不仅是两种互补传感器融合的理论与技术典范，更是极大扩充了视觉SLAM的应用边界。对视觉惯性里程计的研究，其一应该着重构建增强多传感器互补性的理论框架与技术方法，其二应该不断朝着轻量化、高效、可扩展性、高精度与鲁棒性应用的目标前进。

Abstract

本文提出了一种紧耦合、基于滤波框架的双目VIO系统，使用了点、线段特征。在点特征难以被可靠地检测与跟踪的挑战性场景中，线特征的应用能够帮助提高系统的鲁棒性，包括对纹理以及光照变化的鲁棒性。另外，我们提出了一个新的轻量级的基于滤波的闭环算法以降低累积的漂移误差，以代替全局BA优化或者位姿图优化。我们将闭环问题建模为最优地把当前滤波器所维持的滑窗在历史的关键帧中进行重定位的EKF更新问题。本文提供了一个新的视觉惯性数据集，Trifo Ironsides 数据集，该数据由Ironsides传感器采集，其立体相机与IMU数据是高度同步的，数据集包含了多种不同的运动类型和纹理，以及毫米级精度的位姿真值。为了验证所提出系统的性能，本文在EuRoC和Trifo Ironsides数据集上与state-of-the-art方法进行了对比，包括OKVIS，VINS-MONO,以及S-MSCKF。

引言

基于视觉与惯性测量信息的融合时机，VIO可以被分为松耦合与紧耦合两种框架。松耦合分别根据图像和惯性测量观测估计相机位姿，然后融合两个估计以获取最终的状态估计。紧耦合在直接在measurement level融合两种信息以共同估计所有的IMU和相机状态。尽管松耦合是灵活的并且可能更加高效，紧耦合方法能够产生更加精确和鲁棒的运动估计。本文的Trifo-VIO是一种紧耦合方法。
基于视觉测量和惯性测量被融合的方法，VIO方法可以被划分为基于滤波的方法和基于优化的方法。基于滤波的方法使用EKF框架，其中状态预测是通过对IMU测量进行积分实现的，预测校正基于视觉观测。相反地，基于优化的方法使用batch非线性优化直接最小化根据IMU观测积分得到的状态估计与基于最小化冲投影误差得到的相机位姿估计之间的偏差。基于优化的方法一般更加精确但是计算复杂度更好，由于不断地线性化。有的方法将二者的优势结合，比如PIRVS迭代的执行EKF更新步骤用于高效的位姿估计，同时在后端使用优化方法降低漂移误差。本文提出的Trifo-VIO是一种高效的基于滤波的方法，其精度被证明为与目前最好的基于优化的方法在同一个量级，甚至在某些数据上更好。
大多数VIO方法依赖于点特征作为图像观测。这些方法的性能在弱纹理区域，或者光照变化的导致的点特征不能被可靠的检测与跟踪。但是，许多弱纹理场景包含平面元素，其具有丰富的线性特征，而线条的检测对光线变化的敏感性较低。因此，在所提出的Trifo-VIO中，除了点特征，我们提取线段特征作为有效的图像观测以提高挑战性场景下的运动约束，使系统具有较高的鲁棒性。通过使用MSCKF，点特征和线段特征在滑窗内以与特征数目相关的线性复杂度被处理。
另外，视觉和视觉惯性里程计以较高的频率运行但是更加倾向于位姿漂移，因为这些里程计系统不保存一个持续的环境地图。因此，在所提出的Trifo-VIO中，我们介绍了一个轻量级的闭环方法以降低长时间的漂移误差，该方法没有任何计算代价较高的地图优化过程，例如BA。
本文的贡献主要有：

1. 本文是第一个基于滤波的、使用点线特征的、紧耦合双目VIO系统；
2. 本文介绍了一个新的轻量级的基于滤波的闭环方法，被建模为EKF更新。
3. 本文实验对比了多种目前最先进的VIO算法。
4. 本文开放了Trifo Ironsides数据集http://githun.com/TrifoRObotics/IRONSIDES/wiki/Dataset

Related Work

PL-SLAM基于ORB-SLAM框架，对其进行扩展以处理点线匹配关系。双目PL-SALM系统进一步引入基于BoW的场景识别方法，其使用点线描述子。
在基于滤波的VIO方法中，使用边沿或者线条特征的工作较少。其中较早的工作是仅使用线条观测对滤波器进行观测，并且进行能观性的分析。
漂移误差是SLAM中的固有问题。VINS-Mono提出了分为两步的闭环检测方法：(1) 局部紧耦合的重定位将滑窗与历史的位姿相配准；(2) 全局位姿图优化。本文的轻量级闭环检测方法类似于VINS-MONO的闭环方法的第一步，除了本文在滤波框架下实现的并且没有采用全局优化方法以提高效率。

Estimator Description

本文基于MSCKF框架，其核心思想是使用特征跟踪的观测维持并且更新一个相机位子的滑动窗口但是不将特征包含到所估计的状态中。Instead，三维特征位置通过least-squeares multi-view triangulation进行估计，并且在随后被边缘化，在某种程度上类似于无结构的BA。该方法的优势在于能够大幅度降低计算代价，使得MSCKF的计算复杂度与特征数目线性相关，而在EKF-SLAM，其复杂度是特征数目的三次方相关。
本文引入了两种EKF更新策略：(1) 基于联合点线特征的更新；(2) 闭环更新。滤波器的一致性是通过在每次迭代和更新步骤中使用the right mullspace of the observability Gramian改变状态转移矩阵和观测矩阵，参考OC-EKF。
A. 状态参数化表达
定义IMU的状态为：
$$X_B=[{\mathbf{q}}_{BG}^T,{\mathbf{b}}_g^T,{\mathbf{v}}_B^T,{\mathbf{b}}_a^T,{\mathbf{p}}_B^T,{\mathbf{q}}_{BC}^T,{\mathbf{p}}_{BC}^T{]}^T$$
其中，${\mathbf{q}}^T$是从全局坐标系到body坐标系的旋转矩阵的单位四元数表示，,${\mathbf{p}}_B$和${\mathbf{v}}_B$是IMU在全局坐标系下的位置和速度，${\mathbf{b}}_g$和${\mathbf{b}}_a$表示陀螺仪偏置和加速度偏置。可选地，本文将body坐标系到相机的旋转和平移包含进来。
在$k$时刻，估计器的全状态包括当前IMU的状态估计和$N$个相机位姿
$$\hat{\mathbf{X}}=[{\hat{\mathbf{X}}}_{B_k}^T,{\hat{\mathbf{X}}}_{C_1}^T,\dots ,{\hat{\mathbf{X}}}_{C_N}^T{]}^T$$
其中${\hat{\mathbf{X}}}_C=[{\hat{\mathbf{q}}}_{CG}^T,{\hat{\mathbf{p}}}_C^T]$表示相机的位姿估计。
对于误差项，位移、速度以及偏置误差都采用标准的加和形式，对于旋转误差项，采用如下三自由度四元数形式：
$$\delta \mathbf{q}=[\frac{1}{2}\delta \theta ,1{]}^T$$
B. EKF迭代
当获得一个新的IMU观测的时候，其用来对EKF的状态和协方差估计进行迭代。本文使用标准的连续时间IMU运动学模型
$$\begin{gathered} {\dot{\hat{\mathbf{q}}}}_{BG}=\frac{1}{2}\Omega (\hat{w}){\hat{\mathbf{q}}}_{BG} \\ {\dot{\hat{\mathbf{b}}}}_g=0_{3\times 1} \\ {\dot{\hat{\mathbf{v}}}}_B=R({\hat{\mathbf{q}}}_{BG})\hat{\mathbf{a}}+{\mathbf{g}}_G \\ {\dot{\hat{\mathbf{b}}}}_a=0_{3\times 1} \\ {\dot{\hat{\mathbf{p}}}}_B={\hat{\mathbf{v}}}_B \\ {\dot{\hat{\mathbf{q}}}}_{BC}=0_{3\times 1} \\ {\dot{\hat{\mathbf{q}}}}_{BC}=0_{3\times 1} \end{gathered}$$
其中，$\hat{w}$和$\hat{\mathbf{a}}$是不带偏置的角速度和线性加速度。$R$代表四元数对应的旋转矩阵，$\Omega (\hat{w})\in R^{4\times 4}$是一个反对称矩阵
$$\Omega (\hat{w})=\left[\begin{array}{cc}-[\widehat{w_{\times }}]& \hat{w}\\ -{\hat{w}}^T& 0\end{array}\right]$$
离散化采用的是4阶次Runge-Kutta数值方法。具体的状态转移矩阵和协方差矩阵的迭代可参考MSCKF。
C. 点特征的观测模型
在MSCKF中，对同一个3D点的所有的连续观测，例如，特征跟踪，用于对观测到该特征的相机位姿进行更新。残差是标准的冲投影误差
$${\mathbf{r}}_{f_i}={\mathbf{z}}_{f_i}-{\hat{\mathbf{r}}}_{f_i}(12)$$
其中${\mathbf{z}}_{f_i}=[u_i,v_i{]}^T$是图像中第$i$个特征的观测，${\hat{\mathbf{r}}}_{f_i}$是根据其估计的全局三维坐标${\hat{\mathbf{p}}}_{f_i}^G=[{\hat{X}}_i^G,{\hat{Y}}_i^G,{\hat{Z}}_i^G]$投影到所估计的相机位姿对应的相机平面上的坐标观测。重投影模型为
$${\hat{\mathbf{z}}}_{f_i}=\pi (R({\hat{\mathbf{q}}}_{CG})({\mathbf{p}}_{f_i}^G-{\hat{\mathbf{p}}}_C^G))$$
其中，$\pi$是真空相机的投影模型$(\pi :{\mathbb{R}}^3\to {\mathbb{R}}^2)$
$$\pi (p)=\frac{1}{Z}\left[\begin{array}{c}X\\ Y\end{array}\right]$$

我们然后对Eq.(12)进行关于相机位姿估计以及特征位置的线性化，计算对状态的特征坐标的雅克比矩阵分别为$H_{X_{f_i}}$和$H_{f_i}$，具体形式参考MSCKF。然后，本文通过零空间投影将特征位置进行边缘化，以将其与相机位姿进行解耦。
上述介绍的是单目相机的观测模型，其中一个trickly部分就是对三维特征点的位置估计，一般都是通过以least-squares的方式进行多视角的三角化进行计算的。需要多个相机位姿之间具有足够baseline。因此，单目MSCKF不能够对特征的三维位置进行估计，而且EKF在相机静止或者主要做旋转运动的时候不能够更新。因此，本文采用更加实际的双目立体相机以克服该缺陷。对于立体相机的特征观测，本文采用一种简单但是有效的表达方式
$${\hat{\mathbf{z}}}_{f_i}=\left[\begin{array}{c}\pi ({\hat{\mathbf{p}}}_{f_i}^{C_1})\\ \pi ({\hat{\mathbf{p}}}_{f_i}^{C_2})\end{array}\right]=\left[\begin{array}{c}\pi (R({\hat{\mathbf{q}}}_G^{C_1})({\mathbf{p}}_{f_i}^G-{\hat{\mathbf{p}}}_{C_1}^G)\\ \pi (R({\hat{\mathbf{q}}}_G^{C_2})({\mathbf{p}}_{f_i}^G-{\hat{\mathbf{p}}}_{C_2}^G)\end{array}\right]$$
D. 线特征的观测模型
本文将图像的一个直线$l_i$表示为点-法线形式，$l_i=[{\mathbf{z}}_{l_i},{\vec{n}}_{l_i}]$,其中${\mathbf{z}}_i$是线段上的任意一点，${\vec{n}}_{l_i}\in {\mathbb{R}}^{2\times 1}$是表示线段法线方向的单位向量。对于一个三维线段，$L_j$，我们使用两个端点的坐标对进行参数表达，$L_j^G=[{\mathbf{p}}_b^G,{\mathbf{p}}_e^G]$，其中${\mathbf{p}}_b^G,{\mathbf{p}}_e^G$是三维线段的起点和终点坐标。
对于线段特征对应的残差，${\mathbf{r}}_{l_i}\in {\mathbb{R}}^{2\times 1}$，我们使用点到直线的距离
$${\mathbf{r}}_{l_i}=\left[\begin{array}{c}({\mathbf{z}}_{l_i}-{\hat{\mathbf{z}}}_{l_{ib}})\cdot {\vec{\mathbf{n}}}_{l_i}\\ ({\mathbf{z}}_{l_i}-{\hat{\mathbf{z}}}_{l_{ie}})\cdot {\vec{\mathbf{n}}}_{l_i}\end{array}\right](16)$$
其中${\hat{\mathbf{z}}}_{l_{ib}}\in {\mathbb{R}}^{2\times 1}$和${\hat{\mathbf{z}}}_{l_{ie}}\in {\mathbb{R}}^{2\times 1}$是三维线段的起点和终点在二维图像平面上的投影。为了与Eq.(12)相一致，我们将Eq.(16化简为
$${\mathbf{r}}_{l_i}=\left[\begin{array}{c}{\vec{\mathbf{n}}}_{l_i}^T{\mathbf{z}}_{l_i}-{\vec{\mathbf{n}}}_{l_i}^T{\hat{\mathbf{z}}}_{l_{ib}}\\ {\vec{\mathbf{n}}}_{l_i}^T{\mathbf{z}}_{l_i}-{\vec{\mathbf{n}}}_{l_i}^T{\hat{\mathbf{z}}}_{l_{ie}}\end{array}\right]$$
线段特征所提供的有效约束在法线方向。因此，每个被起点和终点表示的3D线段产生一个2D的残差。过参数化能够保证，如果所投影的三维线段和所观测的线段并不是严格匹配的，至少在一个维度其残差不为零。
该观测模型的另外一个好处是其雅克比矩阵很容易计算，特征边缘化的方式与点特征的相同。基于链式法则，我们可以得到该残差对线段特征(起点和终点坐标)的雅克比矩阵如下：
$${\mathbf{H}}_{l_i}=\left[\begin{array}{c}{\vec{\mathbf{n}}}_{l_i}^T{\mathbf{H}}_{l_{ib}}\\ {\vec{\mathbf{n}}}_{l_i}^T{\mathbf{H}}_{l_{ie}}\end{array}\right]$$
其中，${\mathbf{H}}_{l_{ib}}\in {\mathbb{R}}^{2\times 3}$可以按照点特征求解雅克比矩阵的方式进行计算。
相似地，相对于状态的雅克比矩阵为
$${\mathbf{H}}_{X_{l_i}}=\left[\begin{array}{c}{\vec{\mathbf{n}}}_{l_i}^T{\mathbf{H}}_{X_{l_{ib}}}\\ {\vec{\mathbf{n}}}_{l_i}^T{\mathbf{H}}_{X_{l_{ie}}}\end{array}\right]$$
其中，${\mathbf{H}}_{X_{l_{ib}}}$和${\mathbf{H}}_{X_{l_{ie}}}$分别为线段的起点和终点对相机位姿的雅克比矩阵。
将该观测模型可以直接扩展到立体相机观测中
$${\mathbf{r}}_{l_i}=\left[\begin{array}{c}{\vec{\mathbf{n}}}_{l_i,1}^T{\mathbf{z}}_{l_i,1}-{\vec{\mathbf{n}}}_{l_i,1}^T{\hat{\mathbf{z}}}_{l_{ib,1}}\\ {\vec{\mathbf{n}}}_{l_i,1}^T{\mathbf{z}}_{l_i,1}-{\vec{\mathbf{n}}}_{l_i,1}^T{\hat{\mathbf{z}}}_{l_{ie,1}}\\ {\vec{\mathbf{n}}}_{l_i,2}^T{\mathbf{z}}_{l_i,2}-{\vec{\mathbf{n}}}_{l_i,2}^T{\hat{\mathbf{z}}}_{l_{ib,2}}\\ {\vec{\mathbf{n}}}_{l_i,2}^T{\mathbf{z}}_{l_i,2}-{\vec{\mathbf{n}}}_{l_i,2}^T{\hat{\mathbf{z}}}_{l_{ie,2}}\end{array}\right]$$
E. EKF更新：点和线特征
本文采取了和MSCKF相似的更新策略：当一个新的特征不再被跟踪，或者滑窗尺寸超过了预定值，EKF更新被启动。点线特征随后被边缘化。该策略使得算法的计算复杂度与特征数目呈线性关系。边缘化是通过使用特征雅可比矩阵的左零空间，其删除被线性化的残差的特征项。然后我们将transformed 残差和状态对特征的雅可比矩阵放在一起构造最终的残差和观测矩阵。
EKF Update：闭环
当一个新的相机状态加入到滑窗中，我们进行关键帧的选择并且如果被选择为关键帧，则在另外一个线程启动闭环。如果一个闭环被检测到，而该关键帧还在滑动窗口中，那么loop closing updates被启动。否则，关键帧被加入到数据库中。
由于闭环检测简历了当前帧与历史帧之间的特征匹配，我们使用历史关键帧提供的特征位置进行EKF更新，而非使用当前的位姿进行重新三角化，其可能会带来的漂移。
将闭环建模为EKF更新的好处在于漂移能够通过多帧连续帧被逐渐地校正，只要闭环能够被跟踪上。尽管所介绍的闭环更新与[24]和[41]相似，都采用基于地图的更新，但是在这些方法中地图要么被预先构建要么被在线利用BA进行构建，代价较高。

视觉信息处理

点线特征的提取与跟踪与校验。

闭环检测

主要是基于BoW的闭环检测以及特征点的匹配与校验。

实验

数据集
EuRoC MAV Dataset和Trifo Ironsides Dataset

位姿估计结果对比