Register Point Clouds Using Point to Plane ICP

SUMMARY

Register Point Clouds Using Point to Plane ICP aligns two point clouds using the point-to-plane distance metric, optionally with robust kernels for improved convergence on noisy surfaces.

This Skill is useful in industrial, mobile, and humanoid robotics pipelines for precise 6D pose estimation and fine object alignment. For instance, it can refine the alignment of a scanned part to a CAD model in manufacturing, improve map registration for mobile robot navigation, or accurately position objects for humanoid robot grasping. The point-to-plane metric leverages surface normals for faster and more stable convergence compared to point-to-point ICP.

Use this Skill when you want to perform high-accuracy point cloud registration on surfaces with well-defined normals.

The Skill

from telekinesis import vitreous
import numpy as np

registered_point_cloud = vitreous.register_point_clouds_using_point_to_plane_icp(
    source_point_cloud=source_point_cloud,
    target_point_cloud=target_point_cloud,
    initial_transformation_matrix=np.eye(4),
    max_iterations=50,
    max_correspondence_distance=0.05,
    normal_max_neighbors=30,
    normal_search_radius=0.05,
    use_robust_kernel=False,
    loss_type='L2',
    noise_standard_deviation=0.0,
)

API Reference

Performance Note

Current Data Limits: The system currently supports up to 1 million points per request (approximately 16MB of data). We're actively optimizing data transfer performance as part of our beta program, with improvements rolling out regularly to enhance processing speed.

Example

Source and Target Point Clouds

Raw sensor input i.e. target point cloud in green and object model in red

Registered Point Clouds

Registered source point cloud (red) aligned to target point cloud (green) using point-to-plane ICP

The Code

from telekinesis import vitreous
from datatypes import datatypes, io
import pathlib
import numpy as np

# Optional for logging
from loguru import logger

DATA_DIR = pathlib.Path("path/to/telekinesis-data")

# Load point clouds
source_filepath = str(DATA_DIR / "point_clouds" / "weld_clamp_cluster_0_centroid_registered.ply")
target_filepath = str(DATA_DIR / "point_clouds" / "weld_clamp_model_centered.ply")
source_point_cloud = io.load_point_cloud(filepath=source_filepath)
target_point_cloud = io.load_point_cloud(filepath=target_filepath)
logger.success(f"Loaded source point cloud with {len(source_point_cloud.positions)} points")
logger.success(f"Loaded target point cloud with {len(target_point_cloud.positions)} points")

# Execute operation
registered_point_cloud = vitreous.register_point_clouds_using_point_to_plane_icp(
  source_point_cloud=source_point_cloud,
  target_point_cloud=target_point_cloud,
  initial_transformation_matrix=np.eye(4),
  max_iterations=50,
  max_correspondence_distance=0.05,
  normal_max_neighbors=30,
  normal_search_radius=0.05,
  use_robust_kernel=False,
  loss_type='L2',
  noise_standard_deviation=0.0,
)
logger.success("Registered point clouds using point-to-plane ICP")

Running the Example

Runnable examples are available in the Telekinesis examples repository. Follow the README in that repository to set up the environment. Once set up, you can run this specific example with:

cd telekinesis-examples
python examples/vitreous_examples.py --example register_point_clouds_using_point_to_plane_icp

The Explanation of the Code

The script starts by importing the necessary modules for point cloud processing (vitreous), data handling (datatypes, io), file system management (pathlib), numerical operations (numpy), and optional logging (loguru) to track progress.

from telekinesis import vitreous
from datatypes import datatypes, io
import pathlib
import numpy as np

# Optional for logging
from loguru import logger

Next, the source and target point clouds are loaded from disk. Logging confirms that the data has been successfully loaded and shows the number of points in each cloud, providing insight into the size and density of the point clouds. This information helps users select suitable parameters for the registration process.

DATA_DIR = pathlib.Path("path/to/telekinesis-data")

# Load point clouds
source_filepath = str(DATA_DIR / "point_clouds" / "weld_clamp_cluster_0_centroid_registered.ply")
target_filepath = str(DATA_DIR / "point_clouds" / "weld_clamp_model_centered.ply")
source_point_cloud = io.load_point_cloud(filepath=source_filepath)
target_point_cloud = io.load_point_cloud(filepath=target_filepath)
logger.success(f"Loaded source point cloud with {len(source_point_cloud.positions)} points")
logger.success(f"Loaded target point cloud with {len(target_point_cloud.positions)} points")

The main operation applies the register_point_clouds_using_point_to_plane_icp Skill. This method performs point-to-plane ICP (Iterative Closest Point) registration, refining the alignment between source and target point clouds by minimizing the distances from source points to the tangent planes of the target. Key parameters include:

• max_iterations: limits the number of ICP iterations; higher values can improve accuracy but increase computation time.
• max_correspondence_distance: the maximum distance between point pairs to be considered as correspondences; smaller values improve precision but may reject valid matches if the clouds are initially misaligned.
• normal_max_neighbors and normal_search_radius: define the neighborhood used to estimate point normals; tuning these affects the quality of the point-to-plane metric.
• use_robust_kernel and loss_type: options to reduce the effect of outliers for more stable alignment.
• noise_standard_deviation: models expected sensor noise to improve robustness in noisy data.

# Execute operation
registered_point_cloud = vitreous.register_point_clouds_using_point_to_plane_icp(
  source_point_cloud=source_point_cloud,
  target_point_cloud=target_point_cloud,
  initial_transformation_matrix=np.eye(4),
  max_iterations=50,
  max_correspondence_distance=0.05,
  normal_max_neighbors=30,
  normal_search_radius=0.05,
  use_robust_kernel=False,
  loss_type='L2',
  noise_standard_deviation=0.0,
)
logger.success("Registered point clouds using point-to-plane ICP")

This Skill is particularly valuable in industrial 3D perception pipelines, including robot manipulation, part inspection, and object pose estimation. Users can adjust these parameters to balance speed, robustness, and accuracy depending on the characteristics of their point clouds and alignment requirements.

How to Tune the Parameters

The register_point_clouds_using_point_to_plane_icp Skill has several parameters that control the ICP refinement:

max_iterations (default: 50):

• The maximum number of ICP iterations
• Increase (50-200) to allow more refinement
• Decrease (10-30) for faster computation
• Typical range: 10-200
• Use 10-30 for fast, 30-50 for balanced, 50-200 for precision

max_correspondence_distance (default: 0.05):

• The maximum distance to consider points as correspondences
• Units: Uses the same units as your point cloud
• Increase to allow matching more distant points
• Decrease to require closer matches
• Should be 2-5x point spacing
• Typical range: 0.01-0.1 in point cloud units

normal_max_neighbors (default: 30):

• Maximum number of neighbors for normal estimation
• Increase (30-50) to provide smoother normals but is slower
• Decrease (10-30) for faster computation
• Typical range: 10-50

normal_search_radius (default: 0.05):

• Search radius for normal estimation
• Units: Uses the same units as your point cloud
• Increase to consider more neighbors
• Decrease for faster computation
• Should be 2-5x point spacing
• Typical range: 0.01-0.1 in point cloud units

use_robust_kernel (default: False):

• Whether to use robust kernel (e.g., Huber) to reduce outlier influence
• True: Outliers have less impact on alignment
• False: Uses standard L2 loss
• Set to True when point clouds contain outliers or noise

loss_type (default: 'L2'):

• The loss function type
• Options: "L2" (standard), "L1" (robust), "Huber" (balanced)
• "L2" is standard, "L1" is more robust to outliers, "Huber" balances both

noise_standard_deviation (default: 0.0):

• Expected standard deviation of point noise
• Units: Uses the same units as your point cloud
• Increase to give less weight to noisy points
• Typical range: 0.0-0.01 in point cloud units
• Use 0.0 to ignore, 0.001-0.005 for typical noise

TIP

Best practice: Point clouds should have normals for best results. If normals are missing, they will be estimated. Use robust kernel and appropriate loss type when dealing with noisy data or outliers.

Where to Use the Skill in a Pipeline

Point-to-plane ICP is commonly used in the following pipelines:

• Fine alignment refinement
• Precise pose estimation
• Surface-based registration
• Quality inspection

A typical pipeline for fine alignment looks as follows:

# Example pipeline using point-to-plane ICP (parameters omitted).

from telekinesis import vitreous
import numpy as np

# 1. Load point clouds
source_point_cloud = vitreous.load_point_cloud(...)  # Object model
target_point_cloud = vitreous.load_point_cloud(...)  # Scene scan

# 2. Preprocess both point clouds
filtered_source = vitreous.filter_point_cloud_using_statistical_outlier_removal(...)
filtered_target = vitreous.filter_point_cloud_using_statistical_outlier_removal(...)
downsampled_source = vitreous.filter_point_cloud_using_voxel_downsampling(...)
downsampled_target = vitreous.filter_point_cloud_using_voxel_downsampling(...)

# 3. Optional: Initial alignment using Fast Global Registration
initial_aligned = vitreous.register_point_clouds_using_fast_global_registration(...)

# 4. Fine alignment using point-to-plane ICP
registered_cloud = vitreous.register_point_clouds_using_point_to_plane_icp(
    source_point_cloud=downsampled_source,
    target_point_cloud=downsampled_target,
    initial_transformation_matrix=np.eye(4),
    max_iterations=50,
    max_correspondence_distance=0.05,
    normal_max_neighbors=30,
    normal_search_radius=0.05,
    use_robust_kernel=False,
    loss_type='L2',
    noise_standard_deviation=0.0,
)

# 5. Use registered point cloud for pose estimation or analysis

Related skills to build such a pipeline:

• register_point_clouds_using_fast_global_registration: initial alignment before fine ICP
• register_point_clouds_using_point_to_point_icp: alternative ICP method
• filter_point_cloud_using_statistical_outlier_removal: clean input before registration
• filter_point_cloud_using_voxel_downsampling: reduce point cloud density for faster processing

Alternative Skills

Skill	vs. Point-to-Plane ICP
register_point_clouds_using_point_to_point_icp	Use point-to-point ICP when normals are not available or unreliable. Use point-to-plane ICP when normals are available for more accurate registration.
register_point_clouds_using_fast_global_registration	Use FGR for initial coarse alignment. Use point-to-plane ICP for fine refinement.

When Not to Use the Skill

Do not use point-to-plane ICP when:

• Point clouds don't have normals and normal estimation is unreliable (use register_point_clouds_using_point_to_point_icp instead)
• Initial alignment is very poor (use register_point_clouds_using_fast_global_registration first)
• Point clouds are very sparse (normals may not be reliable)
• You need very fast registration (point-to-plane ICP is slower than point-to-point ICP)
• Surfaces are not well-defined (point-to-plane metric works best on smooth surfaces)