Iterative Closest Point

• 又称: ICP, ICP registration
• 类别: State estimation & navigation › Localisation
• 标签: point-cloud, registration, lidar, localization, 3d-perception

一种对齐两个点云的算法，它反复把每个点与另一点云中的最近邻配对，并求解最能拟合这些点对的刚体变换。

定义

迭代最近点（Iterative Closest Point，ICP）是一种配准两组 3D 点的算法，即找出使其中一组与另一组最佳对齐的旋转和平移。它由 Besl 和 McKay 于 1992 年提出，Chen 和 Medioni 大约同一时间也发表了一种密切相关的方法。ICP 是激光雷达里程计、扫描匹配、3D 重建和物体位姿精化的基础工具。

该算法交替执行两个步骤。首先，对源点云中的每个点，在目标点云中找到最近的点，得到一组暂定的对应关系。其次，计算使这些点对之间距离最小的刚体变换；对于点到点距离，这一问题有基于奇异值分解的闭式解。源点云按该变换移动，然后重复这一过程，直到两次迭代之间的变化足够小。

ICP 只能收敛到局部极小值，因此需要一个合理的初始估计，通常来自里程计、IMU 或某种粗略的全局配准方法。如果初始错位很大，最近邻会是错误的，算法可能锁定在一个不正确的对齐上。

许多变体提高了速度和鲁棒性。点到平面 ICP 测量每个点到其匹配点处切平面的距离，在结构化场景中通常收敛更快。Generalized ICP 在两个点云上都对局部表面协方差进行建模。实际实现还会对点进行降采样，使用 k-d 树进行近邻搜索，并剔除相距过远或表面法线不一致的点对。

为何重要

大多数基于激光雷达的定位与建图系统使用 ICP 或其变体来估计机器人在两次扫描之间如何移动，并验证回环。在有物体 3D 模型的情况下，它也是在抓取前精化物体位姿的标准方法。

主要应用

• 移动机器人的激光雷达里程计和扫描匹配
• 在 SLAM 中验证回环
• 根据深度数据精化物体位姿以进行抓取
• 把多次扫描合并成单个 3D 模型
• 把实时传感器数据与先验地图对齐以进行定位

实际应用

为移动机器人构建激光雷达里程计的团队，通常会用体素网格对每次扫描进行降采样，以上一次的运动估计或积分后的 IMU 数据作为初始估计，并针对上一帧扫描或局部地图运行点到平面 ICP。PCL、Open3D 和 libpointmatcher 等库提供了现成的实现。主要的调参项是最大对应距离、收敛阈值和迭代次数上限；工程师会检查最终的拟合度分数，以发现在长走廊或空旷场地等几何特征匮乏处的失败，在那些地方 ICP 可能沿着不受约束的方向滑移。

常见误解

• ICP 能找到全局最优对齐 — 它收敛到局部极小值，需要良好的初始估计
• ICP 适用于任何几何形状 — 在平坦地面或长隧道等退化场景中，某些方向不受约束，结果可能漂移
• ICP 能处理缩放和变形 — 标准 ICP 只估计刚体变换；带缩放或非刚性的对齐需要其他方法

谁需要了解

• 使用激光雷达和深度相机的感知工程师
• SLAM 和定位开发者
• 进行基于模型的物体位姿估计的操作工程师
• 3D 扫描和重建从业者

相关技术

• 点云库（Point Cloud Library，PCL）
• Open3D
• 正态分布变换（Normal Distributions Transform，NDT）
• Generalized ICP
• k-d 树（k-d Tree）
• RANSAC

相关术语

• Point Cloud
• LIDAR
• SLAM
• Transform

相关项目

• Open3D: Modern library for 3D point cloud processing, mesh operations, and 3D machine learning
• oomwoo: Open-source DIY robot vacuum: 3D-printed, ROS2 + 2D LiDAR, fully local.
• PCL: Industry-standard C++ library for point cloud processing in robotics perception
• Cartographer: Google's real-time 2D and 3D SLAM system for multi-platform robotics
• OpenDroneMap: Open-source photogrammetry toolkit that turns drone images into maps and 3D models
• Nav2: ROS 2 navigation stack with path planning and obstacle avoidance for mobile robots

视频

• Iterative Closest Point (ICP) - Computerphile: Computerphile
• Iterative Closest Point (ICP) - 5 Minutes with Cyrill: Cyrill Stachniss