Direct Visual-Inertial Odometry with Stereo Cameras

这对于直接方法是特别有益的：众所周知直接图像对准是非凸的，并且只有在足够准确的初始估计可用时才能预期收敛。虽然在实践中像粗到精跟踪这样的技术会增加收敛半径，但是紧密的惯性积分可以更有效地解决这个问题，因为附加的误差项和先前的结果确保即使对于快速运动也能收敛。我们表明它甚至允许在没有视觉信息的情况下通过短间隔进行跟踪，例如 将相机对准白墙造成的。此外，惯性测量可以观察到全局滚转和俯仰，减少了平移3D运动和偏航旋转的全局漂移。

IMU通常以比相机的帧速率高得多的频率操作，并且可以测量重力方向并消除滚动和俯仰角度的漂移。然而，IMU的缺点是它们仅通过旋转速度和线性加速度间接测量相对姿势。它们噪声很大，需要积分并补偿重力，这在很大程度上取决于姿态估计的准确性。测量带有未知的漂移偏差，需要使用外部参考（如视觉）进行估算。虽然IMU信息是递增的，但是任何图像可以彼此对齐并且具有足够的重叠。这允许在不是直接时间序列的图像之间结合相对姿势测量 - 使得能够进行更一致的轨迹估计。

A. Direct Semi-Dense Stereo Odometry

我们基于Stereo LSD-SLAM进行视觉跟踪[5]：

我们跟踪摄像机朝向地图中的参考关键帧的运动。如果相机距离现有关键帧太远，我们会创建新的关键帧。

我们根据静态和时间双目提示估计当前参考关键帧中的半密集深度图。对于静态立体声，我们利用双目配置中的一对相机之间的固定基线。基于跟踪的运动，从参考关键帧中的像素对应关系向后续图像估计时间立体声。

在跟踪和建图框架中，使用时间立体补充静态有几个好处。静态立体声使重建比例可观察。它也独立于相机移动，但受限于恒定基线，这将静态立体声限制在有效的操作范围内。时间立体声需要非简并的相机移动，但不受[4]中所示的特定范围的约束。该方法可以同时重建非常小和非常大的环境。最后，通过静态与时间立体声的组合，可获得多个基线方向：静态立体声通常具有水平基线 - 其不允许沿水平边缘估计深度，时间立体声允许通过提供其他运动方向来完成深度图。

1）直接图像对准：通过最小化光度残差来估计两个图像I 1和I 2之间的姿势

其中ρ是Huber规范。 使用迭代重新加权的Levenberg-Marquardt算法最小化该目标。

2）深度估计：针对具有高图像梯度的关键帧的像素估计场景几何，因为它们提供稳定的视差估计。

B. IMU Integration

我们的IMU误差函数项的基础是以下非线性动力学模型。

IMU测量通常以比相机帧高得多的频率到达。我们不为每个单独的IMU测量添加独立残差，而是将测量结果集成到图像帧之间的压缩IMU测量中。如果姿势或偏差估计在优化期间发生变化，为了避免频繁重新积分，我们遵循[22]和[14]中提出的预积分方法。我们在IMU坐标系中的时间戳i和j之间积分IMU测量值并获得伪测量值Δpi→j，Δvi→j和R i→j。

我们用Δpi→i = 0，Δvi→i = 0，R i→i = I初始化伪测量，假设IMU测量之间的时间是Δt，我们积分原始测量值：

给定初始状态和综合测量，可以预测下一个时间步的状态：

对于先前状态s i-1和IMU测量帧i和i-1之间的i-1，ωi-1，该方法产生预测

C. Optimization

方程中的误差函数（3）可以写成

权重使用迭代重加权最小二乘法对光度残差r I实现Huber范数，或者对应于IMU残差r IMU的逆协方差（等式（16））。我们使用Levenberg-Marquardt方法优化了这个目标。线性化当前状态周围的残差

误差函数E（s）可以用二次函数近似在当前状态s周围

D. Partial Marginalization

为了约束优化问题的大小，我们执行部分边缘化并将优化状态集保持在小的常量大小。为了约束优化问题的大小，我们执行部分边缘化并将优化状态集保持在小的常量大小。具体地，我们仅针对当前帧状态s i，前一帧s i-1的状态以及用于跟踪的参考帧的状态s ref进行优化。如果我们将状态空间s分成sλ和sμ，其中sλ是我们想要保持在优化中的状态变量，并且sμ是我们想要边缘化的状态的部分，我们可以重写更新 步骤如下

我们发现将Schur补集应用于系统的上半部分

它表示E *（sλ）的系统，其中状态sμ被边缘化。 图3显示了在将每个新帧添加到图形之后应用边缘化过程的图形演变。

E. Changing the Linearization Point

部分边际化固定了sλ的线性化点，用于涉及方程中的sμ和sλ的量。（25）。然而，进一步的优化改变了线性化点，从而需要重新线性化。我们使用一阶近似避免了繁琐的显式重新线性化。如果我们用旧的线性化点sλ和增量Δsλ表示新的线性化点s0λ，

我们可以改变当前E *的二次近似的线性化点

只有当Δsλ和δsλ都很小时才进行近似 - 因为两者都代表状态的更新，这是一个有效的假设。然后我们可以近似在s0λ附近线性化的误差函数：

F. Statistical Consistency

VII. C ONCLUSION

我们提出了一种直接，紧密集成的视觉惯性测距法的新方法。它结合了完全直接的结构和运动方法 - 基于每像素深度而不是单独的关键点观察 - 采用紧密的，基于最小化的IMU集成。我们展示了两个传感器源在理想情况下相互补充：立体视觉允许系统补偿长期IMU偏差漂移，而短期IMU约束有助于克服光度跟踪配方中的非凸性，允许跟踪大型 帧间运动或没有视觉信息的间隔。我们的方法可以在跟踪精度方面优于现有的基于特征的方法，同时提供准确的半密集3D环境重建，同时在标准笔记本电脑CPU上实时运行。