0. 论文信息
标题：LP-ICP: General Localizability-Aware Point Cloud Registration for Robust Localization in Extreme Unstructured Environments
作者：Haosong Yue, Qingyuan Xu, Fei Chen, Jia Pan, Weihai Chen
机构：Beihang University、The Chinese University of Hong Kong、The University of Hong Kong
原文链接：https://arxiv.org/abs/2501.02580
代码链接：https://github.com/xuqingyuan2000/LP-ICP
1. 导读
迭代最近点算法是激光雷达SLAM算法的重要组成部分。然而，在缺乏特征和几何结构的非结构化环境中，它的性能会受到负面影响，导致定位和映射的准确性低和鲁棒性差。众所周知，缺乏几何约束导致的退化会导致沿病态方向的6自由度姿态估计的误差。因此，需要更广泛和更细粒度的退化检测和处理方法。本文提出了一种新的点云配准框架LP-ICP，它结合了ICP算法中点到线和点到平面的距离度量，以及可定位性检测和处理。LP-ICP由可定位性检测模块和优化模块组成。可定位性检测模块通过利用边缘点(具有低局部平滑度)与扫描和地图之间的线以及平面点(具有高局部平滑度)与平面之间的对应性来执行可定位性分析。个体对应约束的可定位性贡献可以应用于更广的范围。优化模块基于可本地化类别向优化方程添加额外的软和硬约束。这允许姿态沿着病态方向被约束，更新趋向于约束值或者保持初始估计不变。这提高了精确度并减少了波动。通过在模拟和真实数据集上的实验，对所提出的方法进行了广泛的评估，证明了比最先进的方法更高或相当的准确性。
2. 效果展示
典型退化环境:来自PLAM数据集的行星状环境和来自CERBERUS DARPA地下挑战数据集的地下隧道环境
融合LP-ICP后的LVI-SAM定位结果：
3. 引言
同时定位与地图构建（SLAM）为机器人提供位姿信息及周围环境地图，是机器人技术中的关键技术。目前，激光雷达（LiDAR）、相机和惯性测量单元（IMUs）等传感器常用于SLAM系统。LiDAR生成的点云能够提供精确的距离测量。因此，基于LiDAR的方法已成为SLAM的一个重要分支。
点云配准是基于LiDAR的SLAM方法中的关键步骤。它通过寻找最佳旋转和平移来对齐两组点云。迭代最近点（ICP）算法常用于扫描到扫描或扫描到地图的配准，以估计机器人的位姿。常见的距离度量包括点对点、点对线和点对平面。这些方法已被广泛应用于许多先进的SLAM系统。近年来，ICP算法继续得到进一步研究。
在典型的城市或室内环境中，现有点云配准方法通常有足够的几何约束。因此，这些环境中的定位精度相当令人满意。然而，极端非结构化环境，如火星或月球上的类行星环境，，以及地下隧道，具有自相似性和稀疏的几何结构。这导致为ICP优化提供的约束不足。在6自由度（6-DoF）位姿的某些方向上，优化会出现退化，导致定位和地图构建的精度降低。
不同的几何约束信息，如不同方向的平面或其边缘，对ICP优化解的影响各不相同。应将此类影响的评估以及不同退化程度的处理纳入框架中，以改善解的性质。Zhang等人首次将退化因子引入状态估计的优化问题中，可用于检测6-DoF位姿每个方向上的总体退化程度。然而，它只能评估给定方向上的总体退化程度，无法计算其中单个约束的可定位性贡献。此外，退化的二元判断依赖于阈值设置，这可能导致约束信息的利用不充分。Tuna等人最近在点对平面ICP的点云配准中引入了单个约束的可定位性检测，并建立了退化的三元判断，称为X-ICP。然而，他们的方法未利用点对线可定位性信息，这限制了进一步提高精度的潜力。而且，用于计算单个约束可定位性贡献的公式无法推广到更广泛的ICP算法及其变体。此外，X-ICP在部分可定位的优化方向中引入了硬约束。这增加了在某些SLAM系统中估计轨迹出现波动甚至显著漂移的风险，可能导致定位失败。
推荐课程：
聊一聊经典三维点云方法，包括：点云拼接、聚类、表面重建、QT+VTK等
。
在本文中，我们提出了一种新的可定位性感知点云配准框架LP-ICP，以解决LiDAR退化问题。该框架旨在提高SLAM系统在极端非结构化环境中的定位和地图构建精度。更具体地说，LP-ICP是一个结合了ICP算法中的点到线和点到平面距离度量的点云配准框架，具有可定位性检测和处理功能。单个对应约束的可定位性贡献的计算公式可应用于ICP的其他变体，或者更广泛地应用于使用高斯-牛顿法优化的位姿估计问题，如通过最小化视觉方法中的重投影误差来估计位姿。因此，它有可能扩展到多传感器融合框架。它由两个模块组成：可定位性检测模块（LocDetect-Module）和具有软硬约束的优化模块（Optimization-Module）。在扫描到地图的配准过程中，LocDetect-module利用边缘点与线之间以及平面点与平面之间的对应关系。它在特征空间的主优化方向上分析贡献强度。然后，我们过滤掉噪声并选择高可定位性贡献。此外，通过三元阈值判断评估6-DoF位姿每个方向的可定位性，分为：完全可定位、部分可定位和不可定位。
优化模块首先利用几何约束和高可定位性贡献对应关系的可定位性信息计算额外的约束。然后，基于LocDetect-Module提供的可定位性类别，向优化方程中添加软硬约束。这确保在非可定位方向上，位姿不更新，保持初始估计不变。在部分可定位方向上，在额外约束下更新位姿，使其趋向于约束值，而不严格等于它。这降低了出现大位姿波动和漂移的风险，否则可能导致定位失败。对于完全可定位的方向，位姿更新不受影响。目的是在尝试确保相对稳定的位姿更新的同时，有效利用更多的几何约束信息。
此外，为了测试算法在典型极端非结构化环境（如类行星环境）中的性能，我们收集了一个包含LiDAR、IMU和相机数据的模拟SLAM数据集，该数据集来自一个视觉逼真的类月环境。数据集包含10个序列，总轨迹长度为10公里。它可用于测试SLAM框架的各个组件以及行星探测中的不同任务。
所提框架已在多个具有挑战性的模拟和真实数据集实验中进行测试。实验表明，LP-ICP在极端非结构化环境中表现良好，能够提升机器人在退化环境中的定位能力和地图构建精度。与最先进的方法相比，所提框架在所有实验中均实现了更高或相当的精度。
4. 主要贡献
本文的贡献如下：
• 我们提出了一种新的具有可定位性检测和处理的ICP算法框架，利用点到线和点到平面的可定位性信息。它用于检测和处理极端非结构化环境中的退化问题。
• 我们提出了一种计算单个对应关系可定位性贡献的通用方法。它可用于使用高斯-牛顿法优化的位姿估计问题。
• 开发了一个结合软硬约束的ICP优化模块，该模块利用可定位性信息来处理退化问题。
• 我们从视觉逼真的类月环境中收集了一个多传感器模拟SLAM数据集，用于在典型的极端退化环境中进行SLAM测试。
• 与最先进的方法相比，多种实验验证了所提框架的有效性。
5. 方法
LP-ICP分为两个模块：可定位性检测模块（LocDetect-Module）和具有软硬约束的优化模块（Optimization-Module）。所提框架如图2所示。LP-ICP被集成到LVISAM[11]的点云配准框架中进行评估和验证。LVI-SAM集成了LIO（LiDAR-惯性里程计）子模块和VIO（视觉-惯性里程计）子模块。LIO子模块是主要组件，VIO子模块为LIO子模块提供初始估计。请注意，在LVISAM中，位姿的3自由度旋转用欧拉角表示。
LocDetect-Module首先推导出计算单个对应约束可定位性贡献的公式。请注意，在ICP算法中，我们将用于约束位姿估计的对应关系（如点到线或点到平面的对应关系）称为单个对应约束。该方法不仅适用于点到线和点到平面ICP算法，还可扩展到使用高斯-牛顿法优化的位姿估计问题，如ICP的变体或通过优化重投影误差进行位姿估计。然后，计算点到线度量中每个边缘点pi在旋转方向上的可定位性贡献向量在平移方向上的可定位性贡献向量。类似地，计算点到平面度量中每个平面点pj在旋转方向上的可定位性贡献向量和在平移方向上的可定位性贡献向量。
将6-DoF位姿每个方向上的可定位性向量聚合起来，以评估相应方向上的可定位性。然后将它们与阈值进行比较，将可定位性分为三类：不可定位（None）、部分可定位（Partial）和完全可定位（Full）。
优化模块利用LocDetect-Module对6-DoF位姿每个方向的分类结果以及较高的可定位性贡献。它构建优化方程来求解最优位姿估计x∗。我们采用结合软硬约束的方法。在非可定位方向上应用硬约束，以防止这些方向上的位姿更新。在部分可定位方向上添加软约束，允许在约束下位姿更新，位姿趋向于约束值，而不严格等于它。这降低了出现大位姿波动或漂移的风险，否则可能导致定位失败。在完全可定位方向上，位姿更新不受影响。
6. 实验结果
7. 总结
为了在具有挑战性的非结构化环境中提高基于激光雷达的SLAM算法的准确性，退化检测和处理是必不可少的。本文提出了一种新的ICP算法框架LP-ICP，它结合了点到线和点到平面的成本函数，以及局部可检测性和处理。LP-ICP通过计算几何对应体的局部可检测性贡献来检测退化，并添加额外的约束以提高姿态估计的准确性。所提出的方法的有效性通过在我们的行星模拟数据集和实际地下隧道数据集上的实验评估得到验证，表明其准确性高于或相当于最先进的方法。
对更多实验结果和文章细节感兴趣的读者，可以阅读一下论文原文~
本文仅做学术分享，如有侵权，请联系删文。
3D视觉交流群，成立啦！
目前我们已经建立了3D视觉方向多个社群，包括
2D计算机视觉
、
最前沿
、
工业3D视觉
、
SLAM
、
自动驾驶
、
三维重建
、
无人机
等方向，细分群包括：
工业3D视觉
：相机标定、立体匹配、三维点云、结构光、机械臂抓取、缺陷检测、6D位姿估计、相位偏折术、Halcon、摄影测量、阵列相机、光度立体视觉等。
SLAM
：视觉SLAM、激光SLAM、语义SLAM、滤波算法、多传感器融合、多传感器标定、动态SLAM、MOT SLAM、NeRF SLAM、机器人导航等。
自动驾驶
：深度估计、Transformer、毫米波|激光雷达|视觉摄像头传感器、多传感器标定、多传感器融合、3D目标检测、路径规划、轨迹预测、3D点云分割、模型部署、车道线检测、Occupancy、目标跟踪等。
三维重建
：3DGS、NeRF、多视图几何、OpenMVS、MVSNet、colmap、纹理贴图等
无人机
：四旋翼建模、无人机飞控等
2D计算机视觉
：图像分类/分割、目标/检测、医学影像、GAN、OCR、2D缺陷检测、遥感测绘、超分辨率、人脸检测、行为识别、模型量化剪枝、迁移学习、人体姿态估计等
最前沿
：具身智能、大模型、Mamba、扩散模型、图像/视频生成等
除了这些，还有
求职
、
硬件选型
、
视觉产品落地、产品、行业新闻
等交流群
添加小助理: cv3d001，备注：
研究方向+学校/公司+昵称
（如
3D点云+清华+小草莓
）, 拉你入群。
▲长按扫码添加助理：cv3d001
3D视觉工坊知识星球
