COIN-LIO: 补偿强度增强的激光雷达惯性里程计

---

---

代码：github
原文：网址
摘要——我们提出了 COIN-LIO，一种激光雷达惯性里程计流程，将激光雷达强度信息与基于几何的点云配准紧密结合。我们的工作重点是提高激光雷达-惯性里程计在几何退化场景（如隧道或平坦区域）中的鲁棒性。我们将激光雷达强度返回值投影到图像中，并提出了一种新颖的图像处理流程，生成在图像内部以及不同场景之间具有更好亮度一致性的滤波图像。我们有效地利用强度作为额外的模态，通过新的特征选择方案，检测点云配准中的无信息方向，并显式选择具有互补图像信息的图像块。图像块中的光度误差最小化随后与惯性测量和点到平面配准在迭代扩展卡尔曼滤波器中融合。所提出的方法在公开数据集上提高了准确性和鲁棒性。我们还发布了一个新的数据集，捕捉了五个真实世界环境中的挑战性、几何退化场景。通过使用额外的光度信息，我们的方法在所有对比的基线方法失效的环境中，显著提高了对几何退化的鲁棒性。

我们的工作主要贡献可以概括如下：

• (1) 我们提出了一个激光雷达强度图像处理流程以及一个几何互补的特征选择方案，使得能够检测和跟踪具有与基于几何测量的互补信息的显著特征。
• (2) 我们表明，我们的方法有效地利用激光雷达强度来提高在几何退化场景中LIO的鲁棒性和性能。
• (3) 我们提供了一个真实世界的数据集ENWIDE，其中包含五个不同几何退化环境的十个序列，并具有准确的位置真值。

我们将我们的贡献整合到一个基于几何的LIO系统中，该系统基于FAST-LIO2，并在一个标准数据集和ENWIDE上展示了优于仅基于几何和基于几何与强度的方法的性能。

图2. 系统概览：输入点云在几何上（绿色）用于地图配准，并作为投影图像（蓝色）用于光度误差最小化。两种残差在迭代更新中结合（橙色）。我们使用配准雅可比矩阵来找到几何中的无信息方向，并选择具有互补图像信息的特征（右下角）。线条表示更新步骤之前（—）和之后（—）的信息流。

一. 方法

COIN-LIO 采用了 FAST-LIO2 中提出的紧密耦合的 iEKF，用于点到平面配准，并将其扩展到光度误差最小化。由于篇幅限制，我们不回顾 FAST-LIO2 [1, 15]，而是专注于光度部分。我们使用一种新颖的滤波器处理点云的强度图像，以提高亮度一致性并减少传感器伪影。我们特别选择图像特征，以在点云几何的无信息方向上提供信息。特征管理模块检查跟踪特征的有效性并检测遮挡。最后，我们将光度残差整合到卡尔曼滤波器中。

A. 定义

我们定义了一个固定全局坐标系 $(G)$，位于 IMU 的初始位置 $(I)$。从激光雷达坐标系 $(L)$ 到 IMU 坐标系的变换假设为已知，表示为 ${\mathbf{T}}_{IL}=({\mathbf{R}}_{IL},{}_I{\mathbf{p}}_{IL})\in SE(3)$。我们定义机器人的状态为 $\mathbf{x}=[{\mathbf{R}}_{GI},{}_G{\mathbf{p}}_{GI},{\mathbf{g}}_I,{\mathbf{b}}^a,{\mathbf{b}}^g,{}_G\mathbf{g}]$，其中 $\mathbf{R}\in SO(3)$ 表示姿态，$\mathbf{p}\in {\mathbb{R}}^3$ 表示位置，$\mathbf{v}\in {\mathbb{R}}^3$ 描述线速度，${\mathbf{b}}^a,{\mathbf{b}}^g\in {\mathbb{R}}^3$ 表示加速度计和陀螺仪的偏置。时间 $t_j$ 时的激光雷达坐标系记为 $L_j$。每个激光雷达扫描由一次完整旋转期间记录的点组成 P = { L j p j , j = 1 , … , k } \mathcal{P} = \{L_j\mathbf{p}_j, j = 1, \ldots, k\} P={ Lj​pj​,j=1,…,k}，其中 $t_j\le t_k$。

B. IMU 预测和点云去畸变

我们根据 FAST-LIO2 [1] 采用卡尔曼滤波器的预测步骤，通过从 $t_j$ 到 $t_k$ 的 IMU 测量积分传播状态。同样，我们计算最新时间戳 $t_k$ 时的自我运动补偿、去畸变点，如下所示：

$$L_k{\mathbf{p}}_j={\mathbf{T}}_{L_k{I}_k}{\mathbf{T}}_{I_k{I}_j}{\mathbf{T}}_{I_j{L}_j}{L}_j{\mathbf{p}}_j.$$

C. 图像投影模型

点 $L_j{\mathbf{p}}_j=[x_j,y_j,z_j]$ 可以通过球面投影投影到图像坐标：

$$c{\mathbf{P}}_j=\Pi (L_j{\mathbf{p}}_j)=\left[\begin{array}{c}{f}_x\varphi +c_x\\ f_y\theta +c_y\end{array}\right]=\left[\begin{array}{c}\frac{-w}{2\pi }\arctan 2\left(\frac{y_j}{x_j},\frac{z_j}{R}\right)+\frac{w}{2}\\ \frac{h}{2\pi }\arcsin \left(\frac{L}{R}\right)+\frac{h}{2}\end{array}\right]=\left[\begin{array}{c}{u}_j\\ v_j\end{array}\right]\text{\hspace{1em}(1)}$$

其中 $R=\sqrt{L^2+z^2}$，$L=\sqrt{x^2+y^2}-r$，如图3所示。垂直视场（FOV）表示为 ${\Theta }_{fov}$，$w$ 和 $h$ 分别表示激光雷达的水平和垂直分辨率。该模型假设后续光束之间的仰角间隔是恒定的。然而，由于制造原因，大多数激光雷达的间隔是不规则的，这会导致球面图像中出现空像素[7]。尽管我们仍然使用公式(1)来计算公式(8)中的雅可比矩阵，但直接使用激光束和编码器值来创建图像。我们像[7]一样补偿水平偏移，但对所有光束使用一个恒定值。我们保留一个从激光雷达校准得到的所有光束仰角的列表 Θ L = { θ 1 , … , θ h } \Theta_L = \{\theta_1, \ldots, \theta_h\} ΘL​={ θ1​,…,θh​}。当我们将一个特征点投影到图像中时，计算 ${\theta }_j$ 并在 ${\Theta }_L$ 中找到上下光束以插值亚像素坐标。

D. 图像处理

光束之间不规则的仰角间隔会在强度图像中导致水平线伪影。在结构丰富的场景中，这些伪影不太明显，但在结构较少的环境中会主导图像。由于它们以规则的行频率出现，我们设计了一个有限冲激响应滤波器来去除它们。首先，我们使用一个截止频率略低于行频率的垂直高通滤波器。除了这些线之外，高通信号还包含该频率处的相关图像内容。因此，我们进一步对它进行水平低通滤波，这将这些线隔离出来，因为相关的图像信号出现在更高的水平频率上。最后，我们从强度图像中减去这个隔离的信号。由于强度值取决于表面的反射率以及距离和入射角，远离传感器的区域强度较低。像Ouster这样的激光雷达还会报告补偿后的反射信号，这在[7]中有所使用，但入射角的影响仍然存在。我们提出了一种不同的方法来实现图像中亮度的一致性。图像中的亮度水平通常会平滑变化，因为平均距离和入射角通常由场景的全局结构驱动，而不是由小的几何细节驱动。因此，我们通过在大窗口中平均强度值来构建亮度图 $I_b(u,v)$。为了实现图像中的一致曝光，我们使用亮度图对像素值进行归一化，并使用常数因子 $s_i$ 将其缩放到 [0, 255] 范围内：

$$I_F(u,v)=s_i\cdot \frac{I(u,v)}{I_b(u,v)+1}\text{\hspace{1em}(2)}$$

最后，我们使用 3×3 高斯核对图像进行平滑处理以减少噪声。我们在图4中提供了示例图像。

图4：（1）强度图像中心过曝，两侧欠曝。（2）我们的滤波图像亮度均匀。（3）和（4）草地和隧道的细节视图。反射率图像欠曝，未显示地面标记（4）。强度图像有线状伪影，影响纹理（3）。滤波器去除了线状伪影（无伪影强度图像）。亮度补偿使曝光均匀，显示远距离细节（4中的地面标记，3中的草地纹理）。

E. 几何互补的图像块选择

我们选择并跟踪 5×5 像素块，这与单个像素相比显示出更好的收敛性[27]。与之前随机选择特征[7]或基于视觉特征检测器[8, 25]的工作不同，我们采用了一种受[28]启发的方法。我们选择图像梯度幅度高于阈值的候选像素，并执行基于半径的非极大值抑制。这种方法不依赖于角点特征，这有利于低纹理图像。候选像素主要检测在 3D 场景中的形状不连续处，例如边缘和角点，或在表面反射率变化处，例如地面标记或植被。在形状不连续处的像素强度雅可比信息通常与点到法线配准中已经捕获的信息重叠。因此，我们旨在选择能够提供额外信息的候选像素，以有效地利用多模态信息。

为了检测点云配准中的无信息方向，我们遵循 X-ICP [11] 中提出的信息分析方法。我们计算点到平面项的 Hessian 矩阵 ${\mathbf{H}}^{ge{o}^T}{\mathbf{H}}^{geo}$ 的主成分。如果累积滤波贡献低于阈值，则检测到一个方向为无信息方向。我们参考读者阅读[11]以获取更多详细信息。我们分析平移分量，并将无信息方向集记为 $V_t$。如果所有方向都是信息性的，我们沿坐标轴插入向量以促进梯度均匀分布。我们计算第二图像矩 $M$ [29]，并使用其最强特征向量 ${\mathbf{v}}_{patch}$ 来近似图像块梯度，这比像素梯度更稳定。然后我们计算如果点沿某个方向扰动，投影图像坐标的变化，使用公式 (7)：

$${\mathbf{d}}_{p_i}=\frac{\partial \Pi (L_j{\mathbf{p}}_j)}{\partial L_j{\mathbf{p}}_j}\cdot {\mathbf{v}}_{t,i}\in {\mathbb{R}}^2,\forall {\mathbf{v}}_{t,i}\in V_t\text{\hspace{1em}(3)}$$

我们选择特征，其中沿无信息 3D 方向移动点会导致 2D 坐标在信息性图像方向上发生变化。因此，我们将投影梯度 ${\mathbf{d}}_{p_i}$ 投影到图像块的信息方向 ${\mathbf{v}}_{patch}$ 上，以计算其方向贡献 $c_i$。由于投影梯度的幅度随着距离的减小而增加，这会倾向于选择靠近传感器的点，因此我们使用归一化梯度：

$$c_i=\frac{{\mathbf{d}}_{p_i}\cdot {\mathbf{v}}_{patch}}{\Vert {\mathbf{d}}_{p_i}\Vert }\text{\hspace{1em}(4)}$$

对于 $V_t$ 中的每个方向，我们选择贡献最强的图像块。我们在图5中展示了结果。

F. 特征管理

我们使用当前位姿估计在全局位置上分别初始化图像块中的每个点。与视觉里程计方法[27]不同，其中为整个图像块分配一个位置，这允许我们分别投影图像块中的每个点。使用高分辨率图像块，我们可以捕捉到细粒度的细节，而以往的工作[7, 17]仅在每个体素网格单元中存储一个值。为了减少计算负载，我们限制了跟踪的图像块数量。在每次更新步骤之后，我们评估特征图像块的有效性。为了检测遮挡，我们比较图像块中每个点的预测距离和测量距离，如果差异超过阈值，则丢弃其中的所有点。我们还会移除低于最小值或高于最大值的图像块。此外，我们计算跟踪图像块和测量图像块之间的归一化互相关（NCC），如果 NCC 低于阈值，则移除该图像块。我们仅在最大帧数内跟踪特征，以减少误差累积并鼓励新特征的初始化。我们通过强制新特征和跟踪特征之间保持最小距离来避免重叠特征。

G. 光度残差和卡尔曼更新

我们最小化跟踪点和当前观测点之间的光度误差。误差通过将跟踪点投影到当前图像中并比较当前强度值与图像块来计算：

$$z^{pho}=I_c(\Pi (L_j{\mathbf{P}}_f))-i_j\text{\hspace{1em}(5)}$$

由于旋转激光雷达依次记录单个点，强度图像中的像素在不同时间和不同位姿下测量。因此，在投影时需要计算跟踪点在失真激光雷达标架 $L_j$ 中的位置：

$$L_j{\mathbf{P}}_f={\mathbf{T}}_{L_j{I}_j}{\mathbf{T}}_{I_j{I}_k}{\mathbf{T}}_{I_{k}GG}{\mathbf{P}}_f\text{\hspace{1em}(6)}$$

然而，这取决于 ${\mathbf{T}}_{I_j{I}_k}$，而 ${\mathbf{T}}_{I_j{I}_k}$ 又取决于未知时间 $t_j$ 本身。RI-LIO 通过在 kD 树中使用 kNN 搜索来解决这个问题。然而，这在低分辨率下才具有计算可行性。因此，我们提出了一种基于投影的解决方案。给定去畸变点云，我们可以近似地知道在哪个时间戳捕获了环境的哪些体素切片。因此，我们通过将去畸变点云投影到图像中并为每个像素分配对应点的索引来构建去畸变映射：$\mathcal{U}(\Pi (L_k{\mathbf{p}}_j))=j$。 为了找到特征点对应的索引，我们将它投影到去畸变映射上，这比 kD 树搜索便宜得多，因此适用于全分辨率点云。给定索引后，我们获取相应的时间戳和 ${\mathbf{T}}_{I_j{I}_k}$ 来计算公式 (6) 和公式 (5)。生成的雅可比矩阵 ${\mathbf{H}}_j^{pho}$ 计算如下：

$${\mathbf{H}}_j^{pho}=\frac{\partial I[C{\mathbf{P}}_j]}{\partial C{\mathbf{P}}_j}\cdot \frac{\partial \Pi (L_j{\mathbf{P}}_j)}{\partial L_j{\mathbf{P}}_j}\cdot \frac{\partial L_j{\mathbf{P}}_j}{\partial \mathbf{x}}\text{\hspace{1em}(7)}$$

$$\frac{\partial I[C{\mathbf{P}}_j]}{\partial C{\mathbf{P}}_j}=\left[\begin{array}{ccc}-\frac{f_{x}y}{x^2+y^2}& \frac{f_{x}x}{x^2+y^2}& 0\\ -\frac{f_{y}xz}{L{R}^2}& -\frac{f_{y}yz}{L{R}^2}& \frac{f_{y}L}{R^2}\end{array}\right]\text{\hspace{1em}(8)}$$

$$\frac{\partial L_j{\mathbf{P}}_j}{\partial \mathbf{x}}=({\mathbf{R}}_{L_j{I}_k}{\mathbf{R}}_{L_{k}I})\left[\begin{array}{ccc}[{\mathbf{R}}_{IG}(c{\mathbf{P}}_j-c{\mathbf{P}}_{GI}){]}_{\times }& -{\mathbf{R}}_{IG}& 0\end{array}\right]\text{\hspace{1em}(9)}$$

$\frac{\partial I[C{\mathbf{P}}_j]}{\partial C{\mathbf{P}}_j}$ 是从邻近像素得到的图像梯度。我们将点到平面 ($geo$) 和光度 ($pho$) 项堆叠成一个组合残差向量 ($\mathbf{z}$) 和雅可比矩阵 ($\mathbf{H}$)。比例因子 $\sigma$ 补偿了几何残差和光度残差之间的不同误差幅度。

$$\mathbf{H}={\left[{\mathbf{H}}_1^{ge{o}^T},\cdots ,{\mathbf{H}}_m^{ge{o}^T},\lambda \cdot {\mathbf{H}}_1^{ph{o}^T},\cdots ,\lambda \cdot {\mathbf{H}}_n^{ph{o}^T}\right]}^T$$

$${\mathbf{z}}_k^{\kappa }={\left[z_1^{geo},\cdots ,z_m^{geo},\lambda \cdot z_1^{pho},\cdots ,\lambda \cdot z_n^{pho}\right]}^T,\mathbf{R}=\mathrm{diag}\left[\sigma \right]$$

我们使用[1]中提供的公式来更新状态：
$$K=({\mathbf{H}}^T{\mathbf{R}}^{-1}\mathbf{H}+{\mathbf{P}}^{-1}{)}^{-1}{\mathbf{H}}^T{\mathbf{R}}^{-1}\text{\hspace{1em}(10)}$$

$${\hat{\mathbf{x}}}_k^{\kappa +1}={\hat{\mathbf{x}}}_k^{\kappa }⊞\left(-K{\mathbf{z}}_k^{\kappa }-(\mathbf{I}-K\mathbf{H})({\mathbf{J}}^{\kappa }{)}^{-1}({\hat{\mathbf{x}}}_k^{\kappa }\boxminus {\hat{\mathbf{x}}}_k)\right)\text{\hspace{1em}(11)}$$