ICP (Iterative Closest Point)
ICP (Iterative Closest Point) – definition
Iterative Closest Point (ICP) is a registration algorithm used to estimate the rigid transformation between two spatial point sets. In mobile robotics, it commonly aligns consecutive 3D LiDAR scans, depth-camera point clouds, or a live scan against a previously built map. The estimated transformation contains translation and rotation and can be used to update a UGV pose.
ICP was formally described by Besl and McKay in an IEEE publication from 1992. The method repeatedly assigns correspondences between points in a source cloud and points in a target cloud, estimates the transformation that minimises alignment error, applies that transformation, and stops when convergence criteria are met.
For a mobile robot, ICP usually estimates a transform between sensor observations in the odom or map frame. It does not replace wheel odometry, IMU integration, GNSS, or SLAM. Instead, it can provide a geometric correction when point-cloud overlap is sufficient and the initial pose estimate is close enough to the correct solution.
How ICP estimates a robot pose
Classic point-to-point ICP minimises the squared Euclidean distance between matched points. Let pi be a source point, qi its corresponding target point, R a rotation matrix, and t a translation vector. The optimisation problem is:
minimise Σ || R p_i + t - q_i ||²The algorithm normally follows these steps:
- Acquire a source point cloud from the current sensor observation.
- Select a target cloud, such as the previous scan or a local submap.
- Use an initial transform from odometry, IMU integration, GNSS, or a previous ICP result.
- Find nearest-neighbour correspondences between source and target points.
- Reject invalid correspondences using distance, normal-angle, or statistical filters.
- Compute an updated rigid transform and repeat until convergence or failure.
Point-to-plane ICP is a common alternative for structured environments. It minimises distance along the normal vector of the target surface rather than full point-to-point distance. It can converge more efficiently on planar surfaces, but it requires reliable surface normals.
ICP in ROS 2 and UGV coordinate frames
ROS 2 does not define ICP as a core navigation interface or a ROS Enhancement Proposal. ICP is typically implemented through libraries such as the Point Cloud Library (PCL), Open3D, or a SLAM package that includes scan matching. ROS 2 nodes commonly exchange 3D clouds through sensor_msgs/msg/PointCloud2.
A mobile robot integration must maintain a valid TF tree. REP 105 defines the conventional frames used by mobile platforms, including map, odom, and base_link. A typical ICP localisation pipeline estimates either odom → base_link motion or a correction between map and odom. Sensor frames, such as lidar_link or camera_link, must be connected to base_link through calibrated static transforms.
ros2 topic info /points_raw
ros2 topic echo /points_raw --once
ros2 run tf2_ros tf2_echo base_link lidar_link
ros2 run tf2_ros tf2_echo map base_linkThe point-cloud timestamp and TF timestamp must be consistent. If the robot moves while a LiDAR scan is acquired, deskewing may also be required. Deskewing uses motion estimates to compensate for points captured at different times within the same scan.
Key ICP parameters and quality metrics
ICP quality depends on the geometry of the scene, sensor noise, overlap between observations, and the initial pose estimate. Parameters should be tuned against recorded robot data rather than copied between sensor configurations.
| Parameter or metric | Purpose in a UGV pipeline |
|---|---|
| Maximum correspondence distance | Rejects matches that are too far apart to represent the same physical surface. |
| Maximum iterations | Limits optimisation runtime and prevents excessive processing time on the onboard computer. |
| Transformation epsilon | Defines the minimum transform change used as a convergence condition. |
| Fitness score or residual error | Measures the remaining alignment error after optimisation. It should be evaluated with correspondence count and scene context. |
| Inlier ratio | Indicates how many proposed correspondences remain after rejection filters. |
| Voxel-grid leaf size | Reduces point density before registration and lowers CPU or GPU load. |
A low residual alone does not prove that the pose is correct. A repetitive corridor, flat wall, crop rows, or an open construction area can produce geometrically plausible but incorrect alignment. ICP should therefore be fused with independent motion or absolute-position measurements where the application requires robust localisation.
Using ICP on Leo Rover and Raph Rover
On Leo Rover, ICP is relevant after adding a point-cloud-producing sensor, such as a 3D LiDAR or a depth camera. Leo Rover uses a differential-drive base. ROS 2 support and compatible distributions depend on the installed Leo Rover software image and packages. Wheel odometry can provide the initial estimate, while an IMU can improve short-term orientation estimation. The resulting ICP correction can be consumed by a localisation or SLAM pipeline, but autonomous navigation requires separate configuration of mapping, localisation, costmaps, planners, and safety behaviour.
The default Raspberry Pi compute unit on Leo Rover makes preprocessing important. Downsampling, range cropping, removal of ground points where appropriate, and local-submap registration reduce the computational cost before ICP. A full dense-cloud registration pipeline may require an external computer depending on sensor data rate, point count, and required update latency.
Raph Rover can carry a larger sensor payload and additional compute hardware for field deployments. ICP can be used for local registration during inspection, agricultural mapping, construction-site documentation, or research experiments. The larger platform does not remove the need for rigid sensor mounting, extrinsic calibration, time synchronisation, and validation on the target terrain.
Limitations and trade-offs
ICP is a local optimisation method. It can converge to a local minimum when the initial transform is poor or when the scan overlap is limited. It also degrades in environments with sparse structure, moving people or vehicles, reflective surfaces, vegetation motion, dust, rain, and repeated geometry.
For 2D laser scanners, scan-matching methods designed for sensor_msgs/msg/LaserScan may be more suitable than 3D ICP. For global relocalisation after a large pose loss, place recognition, GNSS/RTK, fiducial markers, or a global registration method may be needed before local ICP refinement.
Technical references and sources
The following sources document the ICP algorithm, ROS interfaces and coordinate-frame conventions, and a common ICP implementation.
- Besl, P. J. and McKay, N. D., “A Method for Registration of 3-D Shapes,” IEEE Transactions on Pattern Analysis and Machine Intelligence, 1992.
- ROS REP 105 – Coordinate Frames for Mobile Platforms.
- ROS REP 103 – Standard Units of Measure and Coordinate Conventions.
- ROS 2 sensor_msgs/msg/PointCloud2 message definition.
- Point Cloud Library – pcl::IterativeClosestPoint API documentation.