＜论文阅读＞用于 3D 点线和平面的视觉惯性里程计框架 A Visual Inertial Odometry Framework for 3D Points, Lines and Planes

被疫情包围了！在家多看看论文吧～

论文地址：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)=[03×1503×6⋯⌊Cip^j×⌋−WCiRq⋯](2)
公式二中的斜对称矩阵可以使用李群李代数得到，对于一个矩阵 R ∈ S O ( 3 ) R∈SO(3) R∈SO(3) 和一个函数 y = f ( R , x ) = R ⋅ x y=f(R,x) = R \cdot x y=f(R,x)=R⋅x，可以计算得到：
∂ y ∂ R = ∂ ∂ ω ∣ ω = 0 ( e x p ( ω ) ⋅ R ) ⋅ x = − ⌊ y ⌋ × {∂y \over ∂R} = {∂ \over ∂\omega}|_{\omega=0}(exp(\omega)\cdot R) \cdot x = -\lfloor y \rfloor_ \times ∂R∂y=∂ω∂∣ω=0(exp(ω)⋅R)⋅x=−⌊y⌋×其中 ω ω ω 表示 s o ( 3 ) \it {so(3)} so(3) 。
z i ( j ) z^{(j)}_i zi(j) 关于特征位置的雅可比行列式是：
H i ( j ) = W C i R q ‾ (3) H^{(j)}_i = ^{C_i}_WR_{\overline q} \tag{3} Hi(j)=WCiRq(3) 然后堆叠 3D 点 j j j 的所有 M p t M_{pt} Mpt 观测值的残差。给定局部坐标系 C 中 3D 点 j j j 的跟踪对应关系，可以通过取 W p ^ j i , i ∈ 1 , . . . , M p t ^W \hat p^i_j,i ∈ {1,...,M_{pt}} Wp^ji,i∈1,...,Mpt 的平均值轻松确定估计值 W p ^ j ^W\hat p_j Wp^j，，其中 W p ^ j i = ( W C i R q ‾ ) − 1 C i p j ^W \hat p^i_j = (^{C_i}_WR \overline q )^{−1C_i} p_j Wp^ji=(WCiRq)−1Cipj。我们使用 RANSAC 来消除可能不正确的对应关系。

C. 3D 线

除了 3D 点之外，我们的目标还包括 3D 线。在存在噪声观察的情况下，拟合一条线可能会减轻一些噪声。此外，某些场景可能无法提供很多判别点特征，例如，如果几何结构主要由平面组成。我们的目标是对于不同的场景可以稳定的获得 3D 线，引入了各种 3D 线提取技术。为了避免昂贵的搜索，我们只识别那些点位于不同深度值之间的边界上的线，这些线由深度图确定。虽然我们可以使用为 3D 点对应计算的 FPFH 特征，但是对于每个 3D 点，我们计算了局部邻域中的法线，并定义一个类似于 SIFT 特征的基于直方图的特征。我们将具有相似特征的所有点组合在一起，并基于特征分析为它们拟合一条 3D 线 C i L j ^{C_i} L_j CiLj。
作者用起点和终点去参数化一条线。在 3D 线的情况下，我们需要跟踪这些 3D 点并在它们被相机视野遮挡或剪裁时更新它们。为了避免必须为在不同的观察之间的同一条线确定对应锚点，我们仅通过其方向 d d d 参数化一条线。然后将一条线 j j j 转换为：
z i ( j ) = C i L j = f ( w C i R q ‾ , W L j ) = w C i R q ‾ ⋅ W d j z^{(j)}_i = ^{C_i}L_j=f(^{C_i}_wR{\overline q},^WL_j)=^{C_i}_wR{\overline q}\cdot^Wd_j zi(j)=CiLj=f(wCiRq,WLj)=wCiRq⋅Wdj
z i ( j ) z^{(j)}_i zi(j)关于状态和特征位置的雅可比矩阵是：
H X i ( j ) = [ 0 3 × 15 0 3 × 6 ⋯ ⌊ C i d ^ j × ⌋ 0 3 × 3 ⋯ ] (4) H^{(j)}_{X_i} = \begin{bmatrix} 0_{3 \times 15}& 0_{3 \times 6} & \cdots & \lfloor ^{C_i}\hat d_j \times \rfloor & 0_{3 \times 3}& \cdots\\ \end{bmatrix} \tag{4} HXi(j)=[03×1503×6⋯⌊Cid^j×⌋03×3⋯](4) H i ( j ) = W C i R q ‾ (5) H_i^{(j)} = ^{C_i}_WR_{\overline q} \tag{5} Hi(j)=WCiRq(5)然后堆叠 3D 线 j j j 的所有 M l n M_{ln} Mln 观测值的残差。
对于 3D 点，3D 线的估计 W d ^ j ^W{\hat d_j} Wd^j 可以通过对 3D 线 j j j 世界坐标系中的观察进行求平均来得到。不同观测之间的跟踪线是通过比较世界坐标系中的线方向来执行的，并将方向在某个阈值内的那些线进行匹配。然而，这可能潜在地将具有相似方向但在不同 3D 位置的线进行匹配。为了防止这种情况，对于给定的线 L j L_j Lj，我们定义其 3D 点为 P L j = { p 1 , ⋯ , p m } L j P_{L_j}=\lbrace p_1,\cdots,p_m \rbrace_{L_j} PLj={p1,⋯,pm}Lj，首先一条线 P L j < τ P_{L_j} < τ PLj<τ，其中 τ τ τ 是被确定丢弃的内点数的阈值，因为我们凭经验发现它们不能稳健匹配。其次，对于两条线 L j L_j Lj、 L k L_k Lk，我们可以使用关联的 P L j P_{Lj} PLj 和 P L k P_{L_k} PLk 来计算欧几里得距离，如果距离在某个阈值内，则将线 L j L_j Lj、 L k L_k Lk 标记为对于，否则标记为不对应。

D. 3D 平面

最后，我们还考虑了 VIO 的 3D 平面实体。由于点和线的噪声，平面可以进一步减轻不确定性。在这个框架中对 3D 平面的处理遵循线的处理。线和平面之间的主要区别在于我们通过它们的法线方向参数化平面。
z i ( j ) z^{(j)}_i zi(j) 关于状态和特征位置的雅可比矩阵现在是：
H X i ( j ) = [ 0 3 × 15 0 3 × 6 ⋯ ⌊ C i n ^ j × ⌋ 0 3 × 3 ⋯ ] (6) H^{(j)}_{X_i} = \begin{bmatrix} 0_{3 \times 15}& 0_{3 \times 6} & \cdots & \lfloor ^{C_i}\hat n_j \times \rfloor & 0_{3 \times 3}& \cdots\\ \end{bmatrix} \tag{6} HXi(j)=[03×1503×6⋯⌊Cin^j×⌋03×3⋯](6) H i ( j ) = W C i R q ‾ (5) H_i^{(j)} = ^{C_i}_WR_{\overline q} \tag{5} Hi(j)=WCiRq(5)其中 n n n 表示平面法线。然后堆叠 3D 平面 j j j 的所有 M p l M_{pl} Mpl 观测值的残差。
跟踪 3D 平面与 3D 线相同，包括计算组成平面的 3D 点。出于与线相同的稳健性原因（见上文），组成点数量低于阈值的平面被丢弃。

E. 降维

对于世界坐标系下的特征 W f ^Wf Wf，即 3D 点、线或平面，我们有 r ( j ) ≈ H X ( j ) X ~ + H ( j ) W f ~ + n o i s e r^{(j)} \approx H_X^{(j)}\tilde X+H^{(j)} {^W} \tilde f+noise r(j)≈HX(j)X~+H(j)Wf~+noise。为了以等式（1）的方式规定残差，我们执行零空间投影，得到一个特征 j j j 的堆积误差 r o ( j ) r_o^{(j)} ro(j) 和雅可比矩阵 H o ( j ) H_o^{(j)} Ho(j)。在零空间投影之前，Jacobian 是一个 ( 3 M ( j ) × 3 ) (3M^{(j)} × 3) (3M(j)×3) 维矩阵，在投影之后，它的维度是 ( ( 3 M ( j ) − 3 ) × 3 ) ((3M^{(j)}-3) × 3) ((3M(j)−3)×3)，这里 M ( j ) M^{(j)} M(j) 表示实体 j j j 的观察次数：点、线或平面。
如果我们假设 ∀ j ∀j ∀j， M ( j ) = M = M p t = M l n = M p l M^{(j)} = M = M_{pt} = M_{ln} = M_{pl} M(j)=M=Mpt=Mln=Mpl，那么给定 N p t N_{pt} Npt个 3D点， N l n N_{ln} Nln个 3D线和 N p l N_{pl} Npl个 3D平面。通过组合所有实体，堆叠得到的雅可比矩阵是 ( ( N p t + N l n + N p l ) ( 3 M − 3 ) × 3 ) ((N_{pt}+N_{ln}+N_{pl})(3M−3)×3) ((Npt+Nln+Npl)(3M−3)×3) 维的。很明显，通过 QR 分解的降维对于降低计算复杂度是必不可少的。

F. EKF 更新

我们现在使用有用的信息来计算所需的卡尔曼增益以执行 EKF 更新，即确定更新的状态和协方差估计。除了增加 EKF 状态外，新观测值的到来还增加了 EKF 更新所需的协方差矩阵。

五、结论

我们提出了一个统一的框架，将 3D 点、线和平面用于视觉惯性里程计。我们的方法适用于缺乏计算 2D 对应关系的弱纹理的环境，这是大多数先前方法所依赖的。我们的实验表明，通过结合所有三个基元，我们可以提高后续帧之间的配准精度。使用 RGBD 相机获取的深度或点云会受到噪声的影响，跟踪 3D 点可能对这种噪声很敏感。通过额外估计 3D 线和 3D 平面，可以减轻这种噪声。为了避免不得不估计 3D 线和平面的所谓锚点，我们只考虑这些实体的方向。因此，如果我们不能稳健地跟踪 3D 点，我们需要不同方向的 3D 线和平面来约束所有三个维度。在某些环境下，可能没有足够的信息来估计不同方向的 3D 线和平面。在这种情况下，我们总是可以依靠 IMU 进行航位推算，直到再次满足必要条件，但会产生一些漂移。对于未来的工作，我们希望在自主无人机上实现我们的框架，并进一步证明它在纹理较差的环境中的有用性。