被疫情包围！在家看论文~
论文地址：A Visual Inertial Odometry Framework for 3D Points, Lines and Planes
【摘要】:恢复连续相机位置之间的刚性配准是 3D 重建、SLAM 以及视觉里程计的核心。配准依赖于计算连续相机图像中可以区分的 2D 在特征较差的环境(即低纹理和/或低光环境)中，确定特征对应的能力是非常具有挑战性的。本文旨在解决视觉惯性里程计（VIO）恢复连续相机姿势之间复连续相机姿势之间刚性匹配的挑战。除惯性传感外，我们还配备了小型空中机器人 RGBD 相机，并提出了的框架 3D 几何实体的组合：点、线和平面。跟踪的 3D 几何实体对扩展卡尔曼滤波框架提供约束。我们表明，通过直接使用， 3D 我们可以实现几何实体的改进配准。我们在不同纹理差的环境中展示了我们的方法，有些只包含平坦的无纹理表面，基本上不提供跟踪 2D 特征。此外，我们通过比较从运动捕捉系统获得的地面真实姿势轨迹来评估不同的估计姿势轨迹 3D 添加几何实体是否有助于改善姿态估计。我们考虑测试 3D 由于我们的目标是在小型移动机器人我们的目标是在小型移动机器人(如无人机)上实施我们的方法。
一、引言
??里程计和 SLAM 在预先看不见的环境中导航对机器人至关重要。虽然有多种解决方案可用，但它们依靠彩色相机图像中的视觉特征来确定稳定的对应关系。纹理丰富的环境可以提供必要的视觉特征，许多环境都有丰富的纹理，如户外环境。然而，其他环境可能几乎完全缺乏纹理，如楼梯井和电梯井。此外，即使环境纹理丰富，低光水平或雾等大气条件也会对鲁棒的能力产生负面影响。 ??如果相机图像中没有良好的视觉特征，机器人将不得不依靠其他传感方法来提供它 3D 测量配准和姿势估计。激光雷达是一种流行的模式，可以在机器人周围提供 360°点云。然而，激光雷达传感器既昂贵又笨重。在这机器人是无人驾驶飞机 (UAV)激光雷达传感器的重量严重限制了其作为有效载荷的可行性。另一方面，RGBD 相机价格便宜，重量轻，所以得到了 3D 更理想的测量结果选择。尽管当前的 3D 重建和 SLAM 方法结合了 RGBD 它们的深度信息仍然依赖于彩色相机图像的视觉特征。 ??额外的传感器已引入进一步协助 3D 重建和 SLAM，例如惯性测量单元 (IMU)。 IMU 其测量值的漂移是由于偏置不稳定性的缓慢变化和高频噪声的集成。航位计算主要依靠 IMU 随着时间的推移，配准会积累错误，很可能导致重建和建图配准错误。视觉惯性里程计 (VIO) 为了减少或消除这种漂移，漂移。除了 2D 除了特征，最近还考虑了它 VIO 的 2D 线特征。假设环境可以(局部)类似于曼哈顿世界(假设包括三个垂直正交方向)，这种方法依赖于跟踪 2D 线作为相机图像的特征。 ??我们提出了一个 VIO 框架可以处理纹理差的环境，并结合框架 3D 几何实体：来自 RGBD 相机获得的点云 3D 点、3D 线和 3D 平面。3D 点本身可能容易在点云数据中产生噪声，对异常值具有鲁棒性 3D 线与平面的拟合有助于降低对此类噪声的敏感性。由于它依赖于深度测量而不是视觉特征，我们提出的方法的一个主要优点是，它可以在雾、弱光条件或没有视觉纹理的人工结构中有效运行。此外，我们的框架和特定的云获取点 RGBD 相机无关。我们的重点是开发一种可以进入相对较小空间的无人机器人的方法。这限制了无人机的大小，从而限制了其有效载荷的大小和重量。
- 用于检测和跟踪 3D 点云中的 3D 点、3D 线和 3D 平面算法包括跟踪当地相机坐标系 3D 实体估计其在世界坐标系中的位置；
- 提出统一 VIO 将跟踪框架 3D 点、3D 线和 3D 平面作为几何约束合并扩展卡尔曼滤波器 (EKF) 框架中。
三、带有 3D 几何实体的 VIO
??在我们的 VIO 在框架中，视觉信息跟踪的点和线对应关系被合并为几何约束 EKF 中间。可能缺乏纹理或需要确定 2D 对应关系时，我们直接使用基本几何 3D 信息。我们首先简要概述使用情况。 EKF 的符号和 VIO 框架，然后详细描述我们提出的方法。
A. 符号和先决条件
??我们使用以下符号和定义。我们确定了几个坐标空间：惯性传感器的局部坐标系 I I I； RGBD局部坐标系相机 C C C；与世界坐标系 W W W。从空间 A A A 去另一个空间 B B B (刚性)变换 M M M 用 M A B M^B_A MAB 表示。旋转矩阵用 R R R 表示， R q R_q Rq 是通过操作 C ( q ‾ ) C(\overline{q}) C(q)从四元数向量 q ‾ \overline{q} q 获得的。符号 V × V\times V× 表示来自某个 三维向量 V V V 的 3 × 3 3 ×3 3×3 斜对称矩阵。估计量用 x ^ \hat{x} x^ 符号装饰，误差量用 x ~ \tilde{x} x~ 装饰，定义 x ~ = x − x ^ \tilde{x} = x −\hat{x} x~=x−x^ 用于某个量 x x x（例如，状态向量）。
VIO 的目标是在 EKF 估计框架中，根据视觉特征真实估计 IMU 状态。时间 k k k 的 IMU 状态定义为 X I M U k X_{IMU_k} XIMUk。其组成分别是世界和 IMU 坐标系之间的旋转、陀螺仪偏差、世界坐标系中的 IMU 速度、加速度计偏差和世界坐标系中的 IMU 位置。在时间 k k k 的 EKF 状态估计 X ^ k \hat{X}_k X^k，用 N N N 个相机位姿增强 IMU 状态。在 EKF 框架中，状态估计在每个时间步更新如下： X ^ k + 1 = X ^ k + Δ X = X ^ k + K k r k \hat X_{k+1} = \hat X_k + ΔX = \hat X_k + K_kr_k X^k+1=X^k+ΔX=X^k+Kkrk，其中 K k K_k Kk 表示卡尔曼增益矩阵， r k r_k rk 是残差： r k = H X ^ k + n o i s e (1) r_k = H \hat X_k + noise \tag{1} rk=HX^k+noise(1)这里， H H H是测量雅可比矩阵，噪声是零均值高斯白噪声，与状态向量 X X X不相关。 测量模型依赖于从多个位姿观察到静态特征的概念，这对 IMU 的状态估计施加了约束：姿势估计应该与（跟踪的）几何实体的观察结果一致。在方程（1）中，目标是确定测量雅可比矩阵 H H H，我们现在将描述如何将 3D 点、3D 线和 3D 平面的观测值合并到同一个VIO 框架中。
B. 3D 点
给定一对连续的 RGBD 图像，标记为源和目标，我们为点云中的 3D 点计算 Fast Point Feature Histogram (FPFH) 特征。选择FPFH是因为需要较高的计算效率，如果部署在实际无人机上，则需要实时计算。然后我们使用 FPFH 特征来确定源和目标中的 3D 点之间的对应关系。我们将在第 i i i 帧中观察到的 3D 点 j j j 的位置表示为 z i ( j ) = C i p j = W C i R q ‾ ( W p j − W p C i ) z_i^{(j)} = ^{C_i}p_j = ^{C_i}_WR_{\overline q}(^Wp_j -^Wp_{C_i}) zi(j)=Cipj=WCiRq(Wpj−WpCi)。由于 W p j ^W p_j Wpj 是未知的，它是通过跟踪 3D 点对应来估计的。我们将此估计定义为 W p ^ j ^W \hat p_j Wp^j ，并可以估计测量值 z ^ i ( j ) = C i p ^ j = W C i R q ‾ ( W p ^ j − W p C i ) \hat z_i^{(j)} = ^{C_i}\hat p_j = ^{C_i}_WR_{\overline q}(^W\hat p_j -^Wp_{C_i}) z^i(j)=Cip^j=WCiRq(Wp^j−WpCi)，将测量误差定义为： r i ( j ) = z i ( j ) − z ^ i ( j ) r_i^{(j)}=z_i^{(j)}-\hat z_i^{(j)} ri(j)=zi(j)−z^i(j)。 为了在 VIO 测量模型中加入 3D 点，我们更新关于状态的测量 z i ( j ) z^{(j)}_i zi(j) 的雅可比矩阵，如下所示： H X i ( j ) = [ 0 3 × 15 0 3 × 6 ⋯ ⌊ C i p ^ j × ⌋ − W C i R q ‾ ⋯ ] (2) H^{(j)}_{X_i} = \begin{bmatrix} 0_{3 \times 15}& 0_{3 \times 6} & \cdots & \lfloor ^{C_i}\hat p_j \times \rfloor & -^{C_i}_WR \overline q & \cdots\\ \end{bmatrix} \tag{2} HXi(j)=[