ICP还能玩出什么花样？RA-L'24全新GenZ-ICP：根据环境几何特性自适应，解决依赖单一误差度量

单一误差度量的依赖

许多研究者已经研究了基于激光雷达（LiDAR）的里程计，这种方法利用高精度的距离测量并且对光照变化具有鲁棒性。LiDAR里程计采用各种误差度量进行迭代最近点（ICP）匹配，例如点对点和点对平面误差度量，每种方法都有其优缺点。然而，现有的LiDAR（惯性）里程计系统–通常依赖于单一的误差度量，这导致其在不同几何特征的环境中性能有所不同。

例如，尽管点对点误差度量在各种环境中表现良好，但在结构化环境中，它的精度较差，因为它没有利用像表面法线等结构化几何特征。相反，点对平面误差度量在非结构化环境中可能表现得不够精确，因为来自噪声三维信息的法线估计不可靠。为了解决这两个误差度量的局限性，提出了广义ICP（G-ICP），该方法在概率框架中集成了两种误差度量，假设所有周围环境局部平坦。然而，这一假设可能会导致显著的近似误差，具体取决于周围环境的几何特征，进而导致性能退化。

GenZ-ICP[1]是一种全新的ICP方法，以解决仅依赖于单一误差度量的缺点。如图1所示，重新审视了点对平面和点对点误差度量，并提出了一种方法，在不同环境下互补地利用它们的优势。此外，通过利用自适应权重，反映周围环境的几何特性，增强了方法在多样环境中的适应性。

LiDAR里程计的主要退化情况之一是在长廊等结构中常见的一维退化。因此，该方法重点研究了类似长廊的退化情况。

使用自适应权重的可泛化和退化鲁棒LiDAR里程计

应用于LiDAR里程计系统的GenZ-ICP方法，如图2所示。为了实现不同环境下的鲁棒LiDAR里程计，尤其是在退化场景中的鲁棒性，提出了一种由三个关键组件组成的全新ICP方法：a）平面分类，b）点对平面和点对点残差设置，c）基于自适应权重的优化。

问题定义

ICP问题定义为估计最优的刚体变换，使得源点云（来自LiDAR帧L）与目标点云（来自地图帧M）对齐。为了建立每一对点的对应关系，源点云需要根据先前估计的位姿转换到地图帧M中，如图2所示。接下来，需要计算每个对应点对的残差以构建代价函数。最优对齐通过在第n次迭代中增量更新位姿与相对位姿来计算。为了简化说明，以下内容都在第n次迭代中进行描述，因此n在后文中被省略。相对位姿通过求解以下优化问题来估计：

其中，和分别表示估计的旋转矩阵和平移向量，是对应点对的数量，表示L2范数。

通常，残差是通过点对点、点对平面或G-ICP等误差度量来计算的。然而，如前所述，这些误差度量可能会因周围环境的几何特性而导致姿态估计性能的退化。因此，为了克服仅依赖单一误差度量的缺点，我们重新审视了点对平面和点对点误差度量，并提出了一种自适应地结合它们的方式，以在环境变化中实现鲁棒性。

我们将对应点对分为两组，分别计算点对平面残差和点对点残差，具体取决于它们的平面性（见第III-B节）。因此，优化问题（1）可以重写为：

其中，是自适应权重，和分别表示应用点对平面和点对点误差度量的对应点对数量。我们注意到，。自适应权重会根据和之间的比例发生变化（见第III-E节）。为简便起见，、和将在后文中统一表示为、和。

分类平面和非平面结构

如前所述，对于非平面对应点，使用点对平面匹配会将固有的误差传递到代价函数中。因此，为了确保法线估计的可靠性，我们通过分析每个对应点的邻域点分布，将点对对应点进行平面性分类。

然而，当邻域点数量不足时，估计的法线向量可能不够精确。因此，如果邻域点数量小于阈值，则该对应点被视为非平面，并应用不需要法线向量的点对点误差度量。

相反，当邻域点数量大于时，我们通过计算局部表面变化来分析邻域点的分布，该变化定义为：

其中，是通过主成分分析得到的点集的特征值，按降序排列（即）。较低的局部表面变化表示表面平坦且一致，而较高的值表示表面弯曲或不规则。因此，我们通过以下布尔函数来判断是否为平面：

设置点对平面残差和雅可比矩阵

对于平面对应点对，在分类阶段会估计出可靠的法线向量。因此，应用点对平面误差度量[4]，通过点差与法线的点积计算点对平面残差：

其中，和表示旋转矩阵和平移向量，和分别是源点和目标点，是法线向量。通过小角度近似，可以线性化残差：

这里，是将3D向量转换为反对称矩阵的操作。对应的雅可比矩阵为：

其中，。因此，残差可以通过和重写为：

其中，。

设置点对点残差和雅可比矩阵

对于非平面对应点对，我们应用点对点误差度量[3]来计算残差，该残差由变换后的源点与目标点之间的差值确定，如下所示：

为了对该式进行线性化，我们可以使用旋转矩阵的线性化以及反对称矩阵的性质，如下所示：

因此，相应的雅可比矩阵为：

因此，可以重写为：

其中，。

基于自适应权重的优化

在从（7）和（11）计算得到线性化的和后，我们将这些残差整合到代价函数（2）中。然后，为了确定最小化（2）的，我们将其重新格式化为线性最小二乘问题，如下所示：

其中，矩阵和向量分别定义为：

自适应权重在（2）中通过平面对应点对的比例来计算：

因此，代价函数（2）自适应地结合了点对平面和点对点代价函数。如果代价函数仅仅是简单地结合点对平面和点对点代价函数，即，则可能会导致不准确的结果，因为它无法反映周围环境几何特性的变化，如图3(a)所示。相比之下，所提出的方法，如图3(b)所示，采用自适应地调整，以应对变化的环境，能够在结构化和非结构化环境之间自适应切换。最终，求解（12）得到，并且图2所示的增量位姿更新会重复进行，直到变得足够小。

为什么使用自适应权重？：从退化视角的数值分析

我们通过实验演示了我们的方法在类似长廊的退化场景中对优化退化具有鲁棒性，并通过分析条件数[29]提供了其鲁棒性的理论依据。

在缺乏几何特征的环境中，判断当前场景是否为退化情况是非常重要的，而这可以通过退化值来表示。一种表示退化值的方法是条件数，它表示线性系统的数值条件。对于由表示的线性系统，条件数由最大特征值与最小特征值的比值定义：

较高的条件数表示该系统不稳定且数值条件较差。在我们的情况下，对应于（12）中的。

在像长廊这样的退化环境中，退化问题主要出现在位姿的平移部分。因此，我们应用了可观测度量，通过计算的平移部分的条件数来评估退化，计算公式如下：

因此，通过检查（15）中的条件数，我们可以间接识别优化是否发生了退化。

例如，当在长廊中执行点对平面ICP时，由于环境的退化特性，平移部分的Hessian矩阵的条件数会增加。也就是说，法线向量主要沿着两个轴分布，源自地面、天花板和与之垂直的墙壁，从而导致沿长廊方向的姿态模糊。特别地，平行于平移方向的法线向量在迭代过程中往往被排除，因为从远处观察时，由于点云的稀疏性，它们变得不可靠。因此，法线向量的不平衡分布导致沿长廊方向的特征值较小，从而迅速增加条件数，即

然而，我们的自适应权重机制通过对非平面区域的稀疏和噪声点应用点对点误差度量来避免这一问题。与点对平面ICP不同，点对点ICP的平移部分的Hessian矩阵是单位矩阵，从而使得条件数始终保持为1。这表明，数值稳定的优化是可能的，估计值会收敛到最小二乘解。尽管这个解在最小二乘意义上是最优的，但它可能不是全局最优。然而，我们的加权混合方案可以在优化过程中防止姿态发散，从而减少点对平面ICP中常见的大幅姿态漂移。因此，我们的方法通过自适应地结合两种误差度量，展现出较低的条件数，表明它在类似长廊的退化环境中对优化退化具有鲁棒性。

实验效果

总结一下

为了克服依赖单一误差度量的缺点，GenZ-ICP作为一种新型的ICP方法，重新审视了点对平面和点对点误差度量，该方法以互补的方式利用它们的优势。所提出的GenZ-ICP通过根据周围环境的几何特性调整自适应权重，实现了在不同环境中的适应性。该自适应策略在各种环境下表现出鲁棒的姿态估计，尤其在退化场景中优于现有的最先进方法。

未来工作：将致力于将GenZ-ICP应用于激光雷达-惯性里程计框架，以增强在激烈运动下的鲁棒性，从而在多种场景中实现更稳定和更精确的姿态估计。

参考

[1] GenZ-ICP: Generalizable and Degeneracy-Robust LiDAR Odometry Using an Adaptive Weighting