ICP Registration Pipeline Example

SUMMARY

ICP Registration Pipeline Example demonstrates how to align two point clouds using the Vitreous SDK’s Iterative Closest Point (ICP) functions. It covers the full pipeline, including preprocessing, feature estimation, and refinement.

This example is useful in industrial, mobile, and humanoid robotics pipelines for 6D pose estimation, object alignment, and environment mapping. For instance, it can register a scanned part to a CAD model in manufacturing, align consecutive LIDAR frames for mobile robot localization, or merge point clouds from a humanoid robot’s sensors for manipulation tasks.

Use these Skills when you want to accurately align point clouds for perception, pose estimation, or mapping workflows.

The Pipeline

This pipeline focuses on precise alignment of known object models to observed point clouds using a coarse-to-fine registration strategy.

Input Point Cloud
        ↓
Downsample & Preprocess
        ↓
Plane Segmentation & Filtering
        ↓
Subtract Background
        ↓
Cluster Objects
        ↓
Coarse Registration (Centroid Alignment)
        ↓
ICP Refinement
        ↓
Estimated 6D Poses

The output of this pipeline is a set of rigid transformations that precisely align known models to the observed scene.

INFO

This pipeline is commonly applied after object detection, where detected object clusters are registered against known CAD or template models to estimate precise poses.

The Skills

The pipeline is composed of the following Vitreous Skills and operations, listed in the order they are applied:

1. filter_point_cloud_using_voxel_downsampling
   Reduces point cloud density for faster and more stable processing.

2. segment_point_cloud_using_plane
   Detects the dominant planar surface (e.g., table or ground).

3. filter_point_cloud_using_plane_splitting
   Splits the point cloud relative to the detected plane.

4. filter_point_cloud_using_plane_proximity
   Filters points near the plane to remove surface-adjacent noise.

5. subtract_point_clouds
   Removes background or unwanted geometry from the scene.

6. cluster_point_cloud_using_dbscan
   Segments individual object candidates from the scene.

7. convert_mesh_to_point_cloud
   Converts CAD or template models into point clouds for registration.

8. register_point_clouds_using_cuboid_translation_sampler_icp
   Performs coarse alignment between model and scene clusters.

9. register_point_clouds_using_point_to_plane_icp
   Refines alignment to estimate accurate 6D poses.

Examples

This pipeline can be applied to various registration scenarios. The following examples illustrate how the same pipeline behaves across different objects, viewpoints, and sensing conditions.

Example 1: Registering Two Viewpoints

Pipeline Parameters

Voxel Downsampling
voxel_size=2

Plane Segmentation
distance_threshold=1

Plane Splitting
keep_positive_side=True

Plane Proximity Filter
distance_threshold=4

Subtract Point Clouds
distance_threshold=0.1

DBSCAN Clustering
max_distance=20
min_points=50

Point-to-Point ICP
max_iterations=500
max_correspondence_distance=5
min_fitness_score=0.01

Input: Source and Target Point Clouds

Output: Registered Point Clouds

---

Example 2: Multi-Frame Registration

Pipeline Parameters

Voxel Downsampling
voxel_size=2

Plane Segmentation
distance_threshold=1

Plane Splitting
keep_positive_side=True

Plane Proximity Filter
distance_threshold=4

Subtract Point Clouds
distance_threshold=0.1

DBSCAN Clustering
max_distance=20
min_points=50

Point-to-Point ICP
max_iterations=500
max_correspondence_distance=5
min_fitness_score=0.01

Input: Source and Target Point Clouds

Output: Registered Point Clouds

---

Example 3: Colored ICP Registration

Pipeline Parameters

Voxel Downsampling
voxel_size=2

Plane Segmentation
distance_threshold=1

Plane Splitting
keep_positive_side=False

Plane Proximity Filter
distance_threshold=4

Subtract Point Clouds
distance_threshold=0.1

DBSCAN Clustering
max_distance=20
min_points=50

Point-to-Point ICP
max_iterations=500
max_correspondence_distance=10
min_fitness_score=0.01

Input: Source and Target Point Clouds

Output: Registered Point Clouds

The Code

INFO

The runnable implementation for this pipeline will be available soon.

Meanwhile, you’re encouraged to explore the individual Skills to better understand each step of the pipeline.

Alternate Pipeline

If your goal is to detect and localize unknown objects rather than align known models, consider using the Object Detection Pipeline Example.

In many applications, the two pipelines are used together:

• Object Detection Pipeline → segment and localize objects
• ICP Registration Pipeline → estimate precise 6D poses for each detected object