Iterative Closest Point
- Also called: ICP, ICP registration
- Category: State estimation & navigation › Localisation
- Tags: point-cloud, registration, lidar, localization, 3d-perception
An algorithm that aligns two point clouds by repeatedly pairing each point with its nearest neighbor in the other cloud and solving for the rigid transform that best fits those pairs.
Definition
Iterative Closest Point (ICP) is an algorithm for registering two sets of 3D points, that is, finding the rotation and translation that best align one with the other. It was introduced by Besl and McKay in 1992, and a closely related method was published by Chen and Medioni around the same time. ICP is a basic tool for lidar odometry, scan matching, 3D reconstruction and object pose refinement.
The algorithm alternates two steps. First, for each point in the source cloud it finds the closest point in the target cloud, which gives a set of tentative correspondences. Second, it computes the rigid transform that minimizes the distance between those pairs; for point-to-point distances this has a closed-form solution using the singular value decomposition. The source is moved by that transform, and the process repeats until the change between iterations becomes small.
ICP converges only to a local minimum, so it needs a reasonable initial guess, typically from odometry, an IMU or a coarse global registration method. If the starting misalignment is large, the nearest neighbors will be wrong and the algorithm can lock onto an incorrect alignment.
Many variants improve speed and robustness. Point-to-plane ICP measures the distance from each point to the tangent plane at its match, which usually converges faster on structured scenes. Generalized ICP models the local surface covariance on both clouds. Practical implementations also downsample the points, use k-d trees for neighbor search, and reject pairs that are too far apart or have inconsistent surface normals.
Why it matters
Most lidar-based localization and mapping systems use ICP or one of its variants to estimate how the robot moved between scans and to verify loop closures. It is also a standard way to refine an object's pose before grasping when a 3D model of the object is available.
Key applications
- Lidar odometry and scan matching for mobile robots
- Verifying loop closures in SLAM
- Object pose refinement for grasping from depth data
- Merging multiple scans into a single 3D model
- Aligning live sensor data with a prior map for localization
In practice
A team building lidar odometry for a mobile robot typically downsamples each scan with a voxel grid, uses the previous motion estimate or integrated IMU data as the initial guess, and runs point-to-plane ICP against the previous scan or a local map. Libraries such as PCL, Open3D and libpointmatcher provide ready implementations. The main tuning parameters are the maximum correspondence distance, the convergence threshold and the iteration limit, and engineers check the final fitness score to catch failures in geometrically featureless places, such as long corridors or open fields, where ICP can slide along the direction that has no constraint.
Common misconceptions
- ICP finds the globally best alignment — it converges to a local minimum and needs a good initial guess
- ICP works on any geometry — in degenerate scenes such as flat floors or long tunnels some directions are unconstrained and the result can drift
- ICP handles scaling and deformation — standard ICP estimates a rigid transform only; scaled or non-rigid alignment needs other methods
Who should know this
- Perception engineers working with lidar and depth cameras
- SLAM and localization developers
- Manipulation engineers doing model-based object pose estimation
- 3D scanning and reconstruction practitioners
Related technologies
- Point Cloud Library (PCL)
- Open3D
- Normal Distributions Transform (NDT)
- Generalized ICP
- k-d Tree
- RANSAC
Related terms
Related projects
- 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
Videos
- Iterative Closest Point (ICP) - Computerphile: Computerphile
- Iterative Closest Point (ICP) - 5 Minutes with Cyrill: Cyrill Stachniss