VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator-翻译
摘要：本文介绍了一种单目视觉惯性系统（VINS），用于在各种环境中进行状态估计。单目相机和低成本惯性测量单元(IMU)构成了六自由度状态估计的最小传感器套件。我们的算法通过有界滑动窗口迭代地优化视觉和惯性测量，以实现精确的状态估计。视觉结构是通过滑动窗口中的关键帧来维护的，而惯性度量则是通过关键帧之间的预积分来保持的。我们的系统对于未知状态的初始化、在线相机-IMU外参校准、基于球面定义的统一重投影误差、环路检测和四自由度位姿图优化都具有鲁棒性。我们通过与其他最先进的算法进行比较，在公共数据集和真实世界实验中验证了我们系统的性能。我们还在MAV平台上进行了闭环自主飞行，并将算法移植到了iOS应用程序中。我们强调，所提出的工作是一个可靠和完整的系统，可以轻松地在其他智能平台上操作。我们公开了我们的实现代码和一个增强现实（AR）应用程序在iOS移动设备上。
1、概览
所提出的视觉惯性系统的结构如图1所示。
第一部分是测量处理前端，它会为每个新的图像帧提取和跟踪特征，并在两个帧之间预积分所有的IMU数据。第二部分是初始化过程，它提供了必要的初始值（姿态、速度、重力向量、陀螺仪偏差和三维特征位置），以启动非线性系统。第三部分实现非线性图优化，通过优化所有视觉、惯性信息来解决我们滑动窗口中的状态。另一个在另一个线程中运行的部分负责闭环检测和姿态图优化。
符号说明：我们将（·）w 视为世界坐标系，其中重力向量沿着 z 轴。 （·）b 是与 IMU 坐标系对齐的机体坐标系。 （·）c 是相机坐标系。我们使用R和四元数q来表示旋转矩阵。四元数对应于哈密尔顿符号。我们在状态中使用四元数，而在某些方程中，我们使用其对应的旋转矩阵来乘以向量。q wb，pwb是从机体坐标系到世界坐标系的旋转和平移变换。bk是拍摄第k张图像时的机体坐标系，ck是拍摄第k张图像时的相机坐标系。⊗是四元数的乘法运算符。gw = [0, 0, g]T是世界坐标系下的重力向量。
2、测量过程
在本节中，我们对视觉和惯性测量进行预处理。对于视觉测量，我们在连续帧之间跟踪特征，并检测最新帧中的新特征。对于IMU测量，我们在两个连续帧之间进行预积分。需要注意的是，IMU测量受到偏置和噪声的影响。因此，在IMU预积分和优化部分中特别考虑偏差，这对于低成本的IMU芯片至关重要。
A、视觉处理前端
对于每个新图像，使用KLT稀疏光流算法[26]跟踪现有特征。同时，检测新的角点特征[27]以保持每个图像中的最小特征数量（100-300）。该检测器通过设置相邻两个特征之间的最小像素间隔来强制执行统一的特征分布。在通过离群值拒绝之后，特征点被投影到一个单位球上。离群值拒绝是通过基础矩阵测试中的RANSAC步骤来执行的。在这一步中，还会选择关键帧。我们有两个关键帧选择标准。其中之一是平均视差。如果跟踪特征的平均视差超过某个阈值，我们将把该图像视为关键帧。请注意，不仅平移而且旋转也会导致视差；然而，在仅旋转运动中无法三角化特征。为了避免这种情况，我们在计算视差时使用IMU传播结果来补偿旋转。另一个标准是跟踪质量。如果跟踪的特征数量低于某个阈值，我们也将该帧视为关键帧。
B、IMU预积分
我们通过加入IMU偏差校正来扩展我们先前工作中提出的IMU预积分方法[7]。与[28]相比，我们在连续时间动力学中推导出噪声传播。此外，IMU预积分结果还用于初始化过程，以校准初始状态。
假设有两个时间瞬间对应于图像帧 b_k 和 b_{k+1}，在时间间隔 [k, k+1] 内，状态变量受惯性测量的约束限制。
∆t k 是时间间隔 [k, k + 1] 的持续时间。ω̂ t 和 ât 是原始的IMU测量值，它们在本体坐标系中，并受到加速度计偏差 ba、陀螺仪偏差 bw 和噪声的影响。
可以看出，IMU状态传播需要b k 帧的旋转、位置和速度。当这些起始状态发生变化时，我们需要重新传播IMU测量值。特别是在基于优化的算法中，每次调整姿势时，我们都需要重新传播它们之间的IMU测量值。这种传播策略会消耗大量计算资源。为了避免重新传播，我们采用预积分算法。
将IMU传播的参考系改为局部参考系bk后，我们只能预积分与线性加速度â和角速度ω̂相关的部分，具体如下：
"γbkbk" 是初始的单位四元数。可以看出，通过将 bk 作为基础框架，仅使用 IMU 测量值就可以获得预积分部分（4）。αbkbk+1，βbkbk+1和γbkbk+1仅与IMU偏差有关，而与bk和bk+1中的其他状态无关。在开始时，加速度计偏差和陀螺仪偏差都为零。当偏差的估计值发生变化时，如果变化很小，我们通过对偏差的一阶近似进行调整 αbkbk+1，βbkbk+1和γbkbk+1，否则我们进行重新传播。这种策略为基于优化的算法节省了大量的计算资源，因为我们不需要一遍又一遍地传播IMU测量值。
在离散时间实现中，可以应用各种数值积分方法，如欧拉法、中点法、RK4积分等。这里选择欧拉法来演示过程以便易于理解（我们在实现代码中使用中值法）。
一开始，αbkbk，βbkbk为0，γbkbk为单位四元数，表示单位矩阵。均值的传播步骤如下：
i和i+1是对应于[k，k+1]内的两个IMU测量的两个离散时刻。δt是IMU测量i和i+1之间的时间间隔。然后我们处理协方差传播。假设加速度计和陀螺仪测量中的噪声是高斯白噪声，na ∼ N (0, σ2a )，nw ∼ N (0, σ2w )。加速度计偏置和陀螺仪偏置是随机游走，其导数是高斯白噪声，nba ~ N(0, σ2ba)，nbw ~ N(0, σ2bw)。
由于γbkt 是过参数化的，我们定义其误差项为扰动。
我们可以从公式4和公式6推导出误差项的连续时间线性化动力学。
其中，b·c × 是叉积矩阵运算。有关四元数误差状态表示的详细信息可以在文献[29]中找到。Pbkbk+1 可以通过一阶离散时间协方差更新递归计算，初始协方差为 Pbkbk = 0。
其中δt是两个IMU测量之间的时间，Q是噪声的对角协方差矩阵（σ2a，σ2w，σ2ba，σ2bw）。
同时，关于 δzbkbk，δzbkbk+1 的一阶雅可比矩阵 Jbk+1 也可以通过递归计算得出，其中初始雅可比矩阵 Jbk = I。
以递归的方式，我们得到协方差矩阵Pbkbk+1 和雅可比矩阵Jbk+1。关于偏差的一阶近似αbkbk+1，βbkbk+1和γbkbk+1可以写成：
现在我们能够编写带有相应协方差的IMU测量模型Pbkbk+1。
3、初始化
单目紧耦合视觉惯性优化是一个高度非线性的系统。由于单目相机无法直接观测到尺度，因此在没有良好的初始猜测的情况下，直接融合这两个测量是困难的。通常情况下，在假定静止的情况下，前几秒钟的IMU测量平均值被视为重力向量，并且IMU传播被视为初始姿态。然而，当IMU测量受到非平凡偏差的影响或在开始时发生加速运动时，这种处理是不恰当的。为了提高单目视觉惯性系统的成功率，需要进行鲁棒的初始化过程。
我们采用松耦合的传感器融合方法来获取初始值。我们发现，仅基于视觉的SLAM或运动结构（SfM）具有良好的初始化特性。在大多数情况下，仅基于视觉的系统可以通过相对运动方法（例如八点法[30]、五点法[31]、齐次和基础方法）的推导出的初始猜测来引导自己启动。通过将惯性测量单元（IMU）预积分度量与仅视觉结构对齐，我们可以粗略地恢复比例、重力、速度甚至偏差，这有助于引导非线性系统的启动，如图2所示。
A、滑动窗口中的Visual SfM
初始化过程始于一个仅使用视觉信息的结构，该结构估计了一个包含尺度的相机姿态和特征点位置的图。首先，我们检查最新帧和之前帧之间的特征对应关系。如果我们能找到一个之前的帧，它有超过30个跟踪特征，并且最新帧之间的平均视差大于20像素，我们将使用五点法[31]恢复这两个帧之间的相对旋转和尺度。否则，我们将保留窗口中的最新帧并等待新帧。如果五点法成功，我们将固定此平移的比例并三角化在这两个帧中观察到的所有特征。基于这些三角化特征，使用PNP（Perspective-n-Point，PnP）方法来估计整个窗口中其他帧的姿态。最后，应用全局的完整束调整（Bundle Adjustment）[32]来最小化所有特征观测的总重投影误差。由于我们不知道绝对的世界坐标系，因此我们将第一个相机坐标系 (·) c0 设置为基准坐标系。所有坐标系姿态 (p̄c0ck ,qc0ck ) 和特征点位置都是相对于 (·) c0 表示的。根据相机坐标系和IMU（机体）坐标系之间的外参先验（pbc，qbc），我们可以将相机中心的姿态转换为机体中心的姿态。
其中s将视觉结构对齐到绝对比例尺上，这将在接下来的部分中解决。
B、视觉惯性对齐
1）陀螺仪偏置的矫正
考虑窗口中的两个连续帧bk和bk+1，我们可以从视觉结构中得知旋转qc0bk和qc0bk+1，以及从IMU预积分中得到的相对约束γ̂bkbk+1。我们针对陀螺仪偏差线性化了IMU预积分项，并最小化以下方程式的误差：
第一个方程式中，B索引了窗口中的所有帧。第二个方程式是关于陀螺仪偏差的误差状态表示的γ̂ bkbk+1的一阶近似。这样，我们可以获得陀螺仪偏差b_w的初始校准值。然后，我们使用新的陀螺仪偏差重新传播α̂bkbk+1，β̂bkk+1。
2)
速度、重力向量和度量尺度初始化：
我们将在这一步中定义需要估计的变量（速度、重力向量和度量尺度）。
其中vbnbn是在bn时刻下的机体坐标系中的速度，gc0是c0坐标系下的重力向量，s是一个标量。考虑窗口中的两个连续帧bk和bk+1，以bk局部坐标系为基础，位置和速度的IMU测量模型可以表示为：
我们可以将公式13和公式16改写为以下线性形式：
3）重力优化：前一步得到的重力向量可以通过施加大小约束来进行优化。在大多数情况下，重力向量的大小是已知的。然而，如果我们直接将这个大小约束添加到方程（18）的优化问题中，它将变成非线性的，难以解决。在这里，我们使用线性方法通过在其切空间上优化2D误差状态来强制执行此约束。由于重力的大小已知，重力的自由度为两个，我们可以使用其切空间上的两个变量来参数化重力。将重力向量参数化为g ·¯ĝ+ w1 b1 + w2 b2，g是重力的大小，¯ĝ是当前估计的向量的方向，b1和b2是两个正交基，它们张成了切平面。w1和w2分别是向b1和b2的相应位移，如图3所示。我们可以通过叉积轻松地找到一组 b1，b2，如算法1所示。然后，我们将公式17g ·¯ĝ+ w1 b1 + w2 b2中的g替换为相应的值和其在线性形式中。迭代该过程直到ĝ收敛。
在精确计算重力向量后，我们可以通过将重力旋转到 z 轴来获得世界坐标系和 c0 坐标系之间的旋转矩阵qwc0,根据这些旋转矩阵，我们将所有变量从 c0 坐标系旋转到世界坐标系w。在Bode坐标系中的速度也将被旋转到世界坐标系中。此时，初始化过程已经完成，所有这些度量值将被馈送到一个紧密耦合的非线性视觉惯性估计器中。
4、紧耦合非线性优化
在状态初始化之后，我们采用滑动窗口的非线性估计器进行高精度状态估计。图1展示了滑动窗口的示意图。我们使用Ceres Solver [33]来解决这个非线性优化问题。
A、方程
滑动窗口中的完整状态向量定义为（为了表示简单，忽略了转置）：
其中，x_k是第k帧状态，包括在世界坐标系下的位置、速度和方向，以及在机体坐标系下的加速度偏差和陀螺仪偏差。n是关键帧的数量，m是滑动窗口中特征的数量。λ_l是第l个特征从其在单位球上的第一次观测到的逆深度。
我们最小化先验和所有测量残差的马氏距离范数之和，以获得最大后验估计。
B、IMU 模型
考虑窗口中的两个连续帧 bk 和 bk+1，根据 IMU 测量函数公式 12，IMU 测量模型的残差可以定义为：
[.]xyz提取四元素向量的部分，误差状态的近似表示。
C、视觉模型
我们定义在统一单位球上的相机测量残差，适用于小型和大型视场角相机，例如鱼眼和全向相机。考虑第i张图像中首次观察到的第l个特征，第j张图像中观察到的残差被定义为：
Plj 是切空间中固定长度的标准协方差。
D、边缘化
为了限制基于优化的方法的计算复杂度，我们采用边缘化的方法。我们有选择地从滑动窗口中边缘化掉IMU状态xk和特征λl，同时将对应于被边缘化掉的状态的测量转化为先验。
如图5所示，当第二新的帧是关键帧时，它将保留在窗口中，而最旧的帧状态将随其相应的测量值被边缘化掉。否则，如果第二新的帧是非关键帧，则我们会丢弃视觉测量值，而是保留窗口中的IMU测量值，而不是边缘化掉所有测量值。这种策略将保持系统的稀疏性。边缘化方案可以保持空间关键帧在窗口中，同时限制预积分IMU测量的不确定性。
我们基于与已移除状态相关的边际化测量构建了一个新的先验概率。边际化是通过使用舒尔补进行的。直观地说，通过边际化，已移除状态的重要信息被保留，同时计算复杂度得到了限制。
E、 IMU高频输出的传播。
请注意，IMU测量的频率比视觉测量高得多。我们的非线性优化估计器的频率受到视觉测量的限制。为了实时控制的性能，估计器的输出直接与最新的IMU测量一起传播，这作为控制回路中的高频反馈。
F、故障检测和恢复
有时候，由于剧烈的光照变化或极度激烈的运动，失败是不可避免的。积极的故障检测和恢复策略可以提高所提出系统的实用性。故障检测是一个独立的模块，用于检测估计器的不可靠输出。我们有几个故障检测的标准：
- 最新帧跟踪到的特征数量小于5；
- 上两个输出之间的位置或旋转存在巨大的不连续性；
- 偏置或外参参数估计中存在巨大的值；
如果检测到故障，建议的系统将切换到初始状态，尝试重新初始化系统。重新初始化将从最后可能的姿态开始。同时，关键帧数据库将被保留，用于闭环检测。
5、回环检测
我们检测到循环并在循环线程中维护仅姿态的图形。由于滑动窗口缺乏绝对位置和偏航观测，每当发生循环闭合时，滑动窗口将通过重新定位连接到姿态图中。关键帧将在从滑动窗口中删除后添加到姿态图中。当新添加的关键帧包含循环信息时，姿态图将被优化。IMU测量使得滚转和俯仰角度完全可观测，因此累积漂移仅发生在四个自由度（x，y，z和偏航角）。为避免导入虚假信息，我们直接在这四个自由度上优化姿态图。
A、回环检测
我们使用DBoW2 [24]和BRIEF描述符[34]引入的词袋地点识别来执行循环检测。当一个新的关键帧出现时，我们在第II-A节中提取的特征数量不足以进行循环检测，因此我们需要提取更多的特征。我们提取额外的500个FAST角点特征[35]。BRIEF描述符用于描述原始观察到的特征和新检测到的角点特征。这些描述符被视为查询视觉词汇的视觉单词。如果发生循环，DBoW2将在时间和几何一致性检查后返回循环候选项。BRIEF描述符与关键帧一起保留，而原始图像则被丢弃以节省内存。
B、特征检索
当检测到循环时，我们需要通过检索到的特征来建立新关键帧和其循环候选之间的连接。我们只在观察到的特征中查找连接，这些特征具有深度信息。特征检索是通过BRIEF描述符匹配实现的。直接描述符匹配可能会导致很多离群值。为了减少离群值，我们将匹配搜索区域限制在相同位置的邻居范围内。然后，通过基础矩阵测试和RANSAC进行几何一致性检查以去除离群值，如图7所示。当特征对数超过一定阈值时，我们将其视为正确的循环检测，并在接下来的过程中融合其信息。
C、重定位
我们使用m和v来表示新的关键帧及其回环关键帧。由于新的关键帧m将被放入滑动窗口中，而滑动窗口中的所有帧都共享相同的特征，因此我们可以通过这些检索到的特征轻松地将循环关键帧v与滑动窗口连接起来。我们将循环关键帧v联合优化到滑动窗口束中。循环关键帧被视为具有恒定姿态的附加测量帧，仅包含几个视觉约束而没有IMU约束。我们可以很容易地为循环帧v中检索到的特征编写相同的视觉模型，如公式22所示。唯一的区别是循环旧帧的姿态(q̂ wv，p̂ wv)是固定的，这是从先前的里程计中获得的。
将回环项添加到整个非线性成本函数公式（公式20）中。
D、添加关键帧到位姿图
当一个关键帧从滑动窗口中被边缘化时，它将被添加到位姿图中。在实践中，我们每三个关键帧中添加一个关键帧到位姿图中，以限制计算量。这个关键帧在位姿图中作为一个顶点，它将通过两种类型的边连接其他顶点。
1）有序边：一个关键帧将建立到其前面多个关键帧的顺序边。顺序边的物理意义是这两个帧之间的相对变换，该变换是从纯里程计数据中获得的。考虑到新到来的关键帧i和其前一个关键帧j，相对变换仅包含位置p̂iij和偏航角ψ̂ij，在我们的4-DoF姿态图框架中。
2）回环边：
如果新的关键帧检测到循环，它将通过循环边连接循环帧。同样，循环边仅包含这两个帧之间的相对位置和偏航角。计算方式与公式24相同。
E、4-DoF的位姿图优化
由于视觉惯性系统可以完全观测到绝对尺度、横滚角和俯仰角，因此累积漂移只会出现在位置（x、y、z）和偏航角上。为了利用可观测性特征并避免虚假信息，我们仅在循环检测发生时优化4自由度姿态图，而不是6或7自由度优化。我们将帧i和j之间的边的残差定义为最小值：
整个由顺序边和循环边构成的图被优化为：
在这里，S是所有顺序边的集合，L是所有环边的集合。h(·)是Huber鲁棒代价函数。在优化过程中，我们只固定第一个关键帧的姿态为零（p w 0 = 0，φ 0 = 0）。我们使用Huber代价函数来处理环约束的原因是有时会出现误检测的情况。Huber代价函数可以减轻潜在误检测的影响。对于顺序边，我们没有这样的担忧，因为它们是从增量里程计中提取的，没有异常值。
在位姿图优化之后，滑动窗口将会再次与位姿图进行重定位。
F、数据库管理
随着轨迹的增加，数据库变得越来越大。循环检测和姿态优化图的计算时间也变得越来越长。虽然我们采用了最小存储策略（仅保存姿态和特征描述符，不保存原始图像），但是当工作时间长达数小时时，计算时间仍会限制实时性能。为此，我们将数据库维护在有限的大小范围内。当关键帧数量超过一定阈值时，我们根据分布密度对数据库进行下采样。我们删除角度和姿态中心区域内的关键帧，并保留与其邻居之间具有最小距离或角度差异的帧。
6、实验
E：移动设备的使用
为了验证所提出的算法的实用性，我们将整个系统移植到移动设备上，并展示一个简单的增强现实应用程序来展示其准确性和鲁棒性。我们将VINS与Google Tango设备6进行性能比较，后者是当前移动平台上最好的商业增强现实解决方案之一。在这个实验中，我们在iPhone7 Plus上实现了整个VINS系统。我们使用iPhone拍摄的每秒30帧、分辨率为640×480的图像，以及内置的InvenSense MP67B 6轴陀螺仪和加速度计获得的每秒100个IMU数据。如图21所示，我们将iPhone与支持Tango的智能手机Lenovo Phab 2 Pro配对，后者使用全局快门鱼眼相机和同步IMU来估计状态和感知环境。首先，我们在从估计的视觉特征中提取的平面上插入一个虚拟立方体，如图22(a)所示。然后我们拿着两个设备以正常步伐在房间内外行走。当检测到循环时，我们使用4-DoF姿态图优化（第V-E节）来纠正循环旧帧和当前帧之间的所有关键帧的x、y、z和偏航漂移。当我们打开一扇门时，Tango的偏航估计会跳跃一个大角度，如图22(b)所示。原因可能是由于不稳定的特征跟踪或主动故障检测和恢复引起的估计器崩溃。我们的系统在这种具有挑战性的情况下仍然表现良好。在行驶约264米后，我们返回起点位置。整个结果可以在图22(c)中看到，Tango的轨迹在最后一圈中发生漂移，而我们的VINS返回到起点，并通过4-DoF姿态图优化消除了总轨迹中的漂移。这也可以通过立方体在图像上与开始时相同的位置注册来证明。诚然，Tango比所提出的估计更精确。然而，这个实验表明，所提出的方法可以在普通移动设备上运行，并具有与非常专业的设备相比较的潜力。这个实验也证明了所提出的方法的鲁棒性。实验细节可以在补充视频中找到。
7、结论
在这篇论文中，我们实现了一个完整的单目视觉惯性系统，用于6自由度状态估计。我们的系统对于未知状态的初始化、在线相机-IMU外参校准、环路检测和4自由度姿态图优化都具有鲁棒性。这些特点使得我们的系统实用且易于使用。两个应用证明了我们的系统具有很大的潜力，可以扩展到其他平台上。
在未来，我们将基于鲁棒状态估计和来自VINS的稀疏特征进行稠密建图。
R EFERENCES
[1] G. Klein and D. Murray, “Parallel tracking and mapping for small ar
workspaces,” in Mixed and Augmented Reality, 2007. ISMAR 2007. 6th
IEEE and ACM International Symposium on. IEEE, 2007, pp. 225–234.
[2] C. Forster, M. Pizzoli, and D. Scaramuzza, “SVO: Fast semi-direct
monocular visual odometry,” in Proc. of the IEEE Int. Conf. on Robot.
and Autom., Hong Kong, China, May 2014.
[3] J. Engel, T. Schöps, and D. Cremers, “Lsd-slam: Large-scale di-
rect monocular slam,” in European Conference on Computer Vision.
Springer International Publishing, 2014, pp. 834–849.
[4] R. Mur-Artal, J. Montiel, and J. D. Tardos, “Orb-slam: a versatile
and accurate monocular slam system,” IEEE Transactions on Robotics,
vol. 31, no. 5, pp. 1147–1163, 2015.
[5] J. Engel, V. Koltun, and D. Cremers, “Direct sparse odometry,” IEEE
Transactions on Pattern Analysis and Machine Intelligence, 2017.
[6] S. Shen, Y. Mulgaonkar, N. Michael, and V. Kumar, “Initialization-
free monocular visual-inertial estimation with application to autonomous
MAVs,” in Proc. of the Int. Sym. on Exp. Robot., Marrakech, Morocco,
2014.
[7] S. Shen, N. Michael, and V. Kumar, “Tightly-coupled monocular visual-
inertial fusion for autonomous flight of rotorcraft MAVs,” in Proc. of
the IEEE Int. Conf. on Robot. and Autom., Seattle, WA, May 2015.
[8] M. Faessler, F. Fontana, C. Forster, and D. Scaramuzza, “Automatic re-
initialization and failure recovery for aggressive flight with a monocular
vision-based quadrotor,” in Proc. of the IEEE Int. Conf. on Robot. and
Autom. IEEE, 2015, pp. 1722–1729.
[9] Z. Yang and S. Shen, “Monocular visual–inertial state estimation with
online initialization and camera–imu extrinsic calibration,” IEEE Trans-
actions on Automation Science and Engineering, vol. 14, no. 1, pp.
39–51, 2017.
[10] M. Bloesch, S. Omari, M. Hutter, and R. Siegwart, “Robust visual
inertial odometry using a direct ekf-based approach,” in Proc. of the
IEEE/RSJ Int. Conf. on Intell. Robots and Syst. IEEE, 2015, pp. 298–
304.
[11] S. Leutenegger, S. Lynen, M. Bosse, R. Siegwart, and P. Furgale,
“Keyframe-based visual-inertial odometry using nonlinear optimization,”
Int. J. Robot. Research, vol. 34, no. 3, pp. 314–334, Mar. 2014.
[12] Y. Ling, T. Liu, and S. Shen, “Aggressive quadrotor flight using dense
visual-inertial fusion,” in Proc. of the IEEE Int. Conf. on Robot. and
Autom. IEEE, 2016, pp. 1499–1506.
[13] V. Usenko, J. Engel, J. Stückler, and D. Cremers, “Direct visual-inertial
odometry with stereo cameras,” in Proc. of the IEEE Int. Conf. on Robot.
and Autom. IEEE, 2016, pp. 1885–1892.
[14] A. S. Huang, A. Bachrach, P. Henry, M. Krainin, D. Maturana, D. Fox,
and N. Roy, “Visual odometry and mapping for autonomous flight using
an RGB-D camera,” in Proc. of the Int. Sym. of Robot. Research,
Flagstaff, AZ, Aug. 2011.
[15] A. I. Mourikis and S. I. Roumeliotis, “A multi-state constraint Kalman
filter for vision-aided inertial navigation,” in Proc. of the IEEE Int. Conf.
on Robot. and Autom., Roma, Italy, Apr. 2007, pp. 3565–3572.
[16] J. Kelly and G. S. Sukhatme, “Visual-inertial sensor fusion: Localization,
mapping and sensor-to-sensor self-calibration,” Int. J. Robot. Research,
vol. 30, no. 1, pp. 56–79, Jan. 2011.
[17] J. A. Hesch, D. G. Kottas, S. L. Bowman, and S. I. Roumeliotis, “Con-
sistency analysis and improvement of vision-aided inertial navigation,”
IEEE Trans. Robot., vol. 30, no. 1, pp. 158–176, Feb. 2014.
[18] S. Lynen, M. W. Achtelik, S. Weiss, M. Chli, and R. Siegwart, “A robust
and modular multi-sensor fusion approach applied to mav navigation,”
in Proc. of the IEEE/RSJ Int. Conf. on Intell. Robots and Syst. IEEE,
2013, pp. 3923–3929.
[19] M. Li and A. Mourikis, “High-precision, consistent EKF-based visual-
inertial odometry,” Int. J. Robot. Research, vol. 32, no. 6, pp. 690–711,
May 2013.
[20] G. Sibley, L. Matthies, and G. Sukhatme, “Sliding window filter with
application to planetary landing,” J. Field Robot., vol. 27, no. 5, pp.
587–608, Sep. 2010.
[21] C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “IMU prein-
tegration on manifold for efficient visual-inertial maximum-a-posteriori
estimation,” in Proc. of Robot.: Sci. and Syst., Rome, Italy, Jul. 2015.
[22] A. Martinelli, “Closed-form solution of visual-inertial structure from
motion,” Int. J. Comput. Vis., vol. 106, no. 2, pp. 138–152, 2014.
[23] J. Kaiser, A. Martinelli, F. Fontana, and D. Scaramuzza, “Simultaneous
state initialization and gyroscope bias calibration in visual inertial aided
navigation,” IEEE Robotics and Automation Letters, vol. 2, no. 1, pp.
18–25, 2017.
[24] D. Gálvez-López and J. D. Tardós, “Bags of binary words for fast
place recognition in image sequences,” IEEE Transactions on Robotics,
vol. 28, no. 5, pp. 1188–1197, October 2012.
[25] H. Strasdat, J. Montiel, and A. J. Davison, “Scale drift-aware large scale
monocular slam,” Robotics: Science and Systems VI, 2010.
[26] B. D. Lucas and T. Kanade, “An iterative image registration technique
with an application to stereo vision,” in Proc. of the Intl. Joint Conf. on
Artificial Intelligence, Vancouver, Canada, Aug. 1981, pp. 24–28.
[27] J. Shi and C. Tomasi, “Good features to track,” in Computer Vision
and Pattern Recognition, 1994. Proceedings CVPR’94., 1994 IEEE
Computer Society Conference on. IEEE, 1994, pp. 593–600.
[28] C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “On-manifold
preintegration for real-time visual-inertial odometry,” IEEE Transactions
on Robotics (TRO), 2016.
[29] N. Trawny and S. I. Roumeliotis, “Indirect kalman filter for 3d attitude
estimation,” University of Minnesota, Dept. of Comp. Sci. & Eng., Tech.
Rep, vol. 2, p. 2005, 2005.
[30] A. Heyden and M. Pollefeys, “Multiple view geometry,” Emerging
Topics in Computer Vision, 2005.
[31] D. Nistér, “An efficient solution to the five-point relative pose problem,”
IEEE transactions on pattern analysis and machine intelligence, vol. 26,
no. 6, pp. 756–770, 2004.
[32] B. Triggs, P. F. McLauchlan, R. I. Hartley, and A. W. Fitzgibbon,
“Bundle adjustmenta modern synthesis,” in International workshop on
vision algorithms. Springer, 1999, pp. 298–372.
[33] S. Agarwal, K. Mierle, and Others, “Ceres solver,” http://ceres-solver.
org.
[34] M. Calonder, V. Lepetit, C. Strecha, and P. Fua, “Brief: Binary robust
independent elementary features,” Computer Vision–ECCV 2010, pp.
778–792, 2010.
[35] E. Rosten and T. Drummond, “Machine learning for high-speed corner
detection,” Computer vision–ECCV 2006, pp. 430–443, 2006.
[36] M. Burri, J. Nikolic, P. Gohl, T. Schneider, J. Rehder, S. Omari, M. W.
Achtelik, and R. Siegwart, “The euroc micro aerial vehicle datasets,”
The International Journal of Robotics Research, 2016.
[37] C. Mei and P. Rives, “Single view point omnidirectional camera cal-
ibration from planar grids,” in Robotics and Automation, 2007 IEEE
International Conference on. IEEE, 2007, pp. 3945–3950.
[38] L. Heng, B. Li, and M. Pollefeys, “Camodocal: Automatic intrinsic
and extrinsic calibration of a rig with multiple generic cameras and
odometry,” in Intelligent Robots and Systems (IROS), 2013 IEEE/RSJ
International Conference on. IEEE, 2013, pp. 1793–1800.
