Point Cloud Registration, Done Right

A point cloud is a universe filled with noise. Yet the true structure is always there.

Intro

I recently had the opportunity to implement a SLAM correction algorithm for the construction industry. Along the way, I worked through the fundamentals of point cloud registration from scratch — and this article is my attempt to share that process. It covers the math, algorithms, and visualizations I needed to understand to get the implementation right.

In 3D scanning and SLAM, correctly aligning point clouds captured from different viewpoints is a fundamental problem.

In this Notebook, we will understand the basic pipeline of Point Cloud Registration step by step using mathematical formulas, algorithms, and visualizations.

Key concepts you will experience:

• Local shape feature extraction (FPFH) from point clouds
• How to create correspondences from features
• Rigid transformation estimation with TEASER++, which is robust to outliers
• Precise alignment with ICP
• Similarity evaluation (Overlap / Chamfer) between point clouds

Prerequisites (Installation)

Example:

pip install open3d numpy scikit-learn matplotlib

0. Understanding the Math (Rigid Transformations) at a Minimum

When moving a 3D point (p∈R³) using rotation (R) and translation (t):

• (R) is the rotation matrix (preserves lengths and angles)
• (t) is the translation vector

If point (pi) in point cloud A and point (qi) in point cloud B "refer to the same location (are corresponding points)":

If All Correspondences Are Correct (Ideal Case)

We can estimate (R, t) using the following least squares:

But in Reality…

Correspondence points often contain many incorrect matches (outliers) (especially in SLAM-derived point clouds).

This is where TEASER++ comes in:

• Even when many correspondence points are incorrect (high outlier rate)
• It estimates the correct (R,t) as reliably as possible

In this notebook, you will experience where TEASER++ is effective and why it is useful.

1. Setup (Importing TEASER++)

You need to be able to import the TEASER++ Python binding teaserpp_python.

We add .../TEASER-plusplus/python from the build location of TEASER++ to sys.path to import it.

Please adjust TEASER_PY_PATH below to match your environment.

import os, sys
import numpy as np
import matplotlib.pyplot as plt
import open3d as o3d

# Set this to your TEASER-plusplus/python build path
# e.g. "/path/to/TEASER-plusplus/python"
TEASER_PY_PATH = ""
if TEASER_PY_PATH and TEASER_PY_PATH not in sys.path:
    sys.path.append(TEASER_PY_PATH)

import teaserpp_python

print("Open3D version:", o3d.__version__)
print("TEASER++ imported successfully")

Open3D version: 0.19.0
TEASER++ imported successfully

2. Loading Sample Point Clouds (Open3D Demo Data)

Open3D includes two point clouds for ICP (Iterative Closest Point) demonstration:

• source: The moving side (transformation is applied to this)
• target: The reference side (fixed)

The goal is to "align" these two point clouds.

pcd_data = o3d.data.DemoICPPointClouds()
source = o3d.io.read_point_cloud(pcd_data.paths[0])
target = o3d.io.read_point_cloud(pcd_data.paths[1])

print("source points:", len(source.points))
print("target points:", len(target.points))

source points: 198835
target points: 137833

If you want to see the raw point clouds:

o3d.visualization.draw_geometries([source], window_name="source (raw)")
o3d.visualization.draw_geometries([target], window_name="target (raw)")

What is Point Cloud Data?

A point cloud (Point Cloud) is a collection of points in 3D space.

Each point has the following coordinates:

In other words, a point cloud is a list of 3D coordinates like:

(x, y, z)
(x, y, z)
(x, y, z)
...

File Formats

Point clouds are saved in the following formats:

• PLY — Stanford Point Cloud Format
• PCD — Point Cloud Library format
• LAS — LiDAR data
• XYZ — Simple coordinate text

Open3D's demo uses **.pcd** format.

Internal Representation in Open3D

In Open3D, point clouds are treated as Nx3 arrays:

[[x1 y1 z1]
 [x2 y2 z2]
 [x3 y3 z3]
 ...
]

Let's verify this.

import numpy as np

pts = np.asarray(source.points)

print("shape:", pts.shape)
print("first 5 points:")
print(pts[:5])

shape: (198835, 3)
first 5 points:
[[1.01953125 0.88671875 2.27726722]
 [1.04296875 0.88671875 2.27699804]
 [1.05859375 0.88671875 2.27722216]
 [1.08472502 0.88671875 2.26953125]
 [1.08203125 0.88671875 2.27696276]]

This means this point cloud consists of:

198835 (x,y,z) points

In this Notebook, we prepare:

source  = the moving point cloud
target  = the reference point cloud

and our goal is to apply:

rotation + translation to source

to align it with the target (registration).

3. Preprocessing: Downsampling + Normal Estimation + FPFH (Open3D)

Why is Preprocessing Necessary?

• Many points slow down computation → Voxel downsampling reduces the number of points
• Computing FPFH requires normals (surface orientation)
• FPFH is a feature descriptor (33 dimensions) representing "local shape", used for correspondence search

Voxel Downsampling Algorithm

In voxel downsampling, the space is divided into small cube (voxel) grids, and the points within each voxel are replaced by a single representative point.

The algorithm is as follows:

1. Divide the space into a cubic grid of size voxel_size
2. Collect the points contained in each voxel
3. Calculate their average position (centroid)
4. Keep that average point as the representative point

Expressed as a formula:

This is a process of merging points in the same small region into one.

The benefits are:

• Significantly fewer points
• The overall shape structure is preserved

def preprocess_point_cloud(pcd: o3d.geometry.PointCloud, voxel_size: float):
    # 1) Downsample
    pcd_down = pcd.voxel_down_sample(voxel_size)

    # 2) Normal estimation
    pcd_down.estimate_normals(
        o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size * 2.0, max_nn=30)
    )

    # 3) FPFH features
    fpfh = o3d.pipelines.registration.compute_fpfh_feature(
        pcd_down,
        o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size * 5.0, max_nn=100),
    )
    return pcd_down, fpfh

voxel_size = 0.05
src_down, src_fpfh = preprocess_point_cloud(source, voxel_size)
tgt_down, tgt_fpfh = preprocess_point_cloud(target, voxel_size)

print("src_down:", np.asarray(src_down.points).shape)
print("tgt_down:", np.asarray(tgt_down.points).shape)
print("FPFH shape (33 x N):", np.asarray(src_fpfh.data).shape, np.asarray(tgt_fpfh.data).shape)

src_down: (4760, 3)
tgt_down: (3440, 3)
FPFH shape (33 x N): (33, 4760) (33, 3440)

Visualizing Downsampling

The following code compares:

• The original point cloud
• The downsampled point cloud

in more detail, for understanding.

voxel_size = 0.05

source_down = source.voxel_down_sample(voxel_size)

print("Original points:", np.asarray(source.points).shape[0])
print("Downsampled points:", np.asarray(source_down.points).shape[0])

Original points: 198835
Downsampled points: 4760

source_temp = source.paint_uniform_color([1, 0, 0])
source_down_temp = source_down.paint_uniform_color([0, 1, 0])

print("Red = original point cloud")
print("Green = downsampled point cloud")

o3d.visualization.draw_geometries([source_temp])
o3d.visualization.draw_geometries([source_down_temp])

Red = original point cloud
Green = downsampled point cloud

Intuitive Image

Original point cloud:

• • • • •
 • • • •
• • • • •

Voxel division:

□ □ □
□ □ □
□ □ □

Average per voxel:

•   •   •
   •   •
•   •   •

In other words:

Dense point cloud → Representative points

The data is compressed.

Normal Estimation

Why Are Normals Needed?

FPFH creates features using the angular relationships between normals of surrounding points. Therefore, we first need to estimate the normal (surface normal) at each point.

Why Normals Can Be Computed Without an Origin

Normals are determined by the local shape, not the absolute position of coordinates.

Approach:

1. Select a point (p)
2. Collect its k neighboring points
3. Find the plane that best fits those points
4. The vector perpendicular to that plane is the normal

Mathematical Details (Simplified)

Given neighboring points:

First, compute the mean:

Then construct the covariance matrix:

Eigenvalue decomposition of this matrix gives:

• Larger eigenvalues → Plane direction
• Smallest eigenvalue → Normal direction

radius = voxel_size * 2

source_down.estimate_normals(
    o3d.geometry.KDTreeSearchParamHybrid(
        radius=radius,  # within radius
        max_nn=30       # maximum 30 points
    )
)

o3d.visualization.draw_geometries(
    [source_down],
    point_show_normal=True
)

As shown, the normal at each point is computed by estimating the local plane that best fits the neighboring points, and taking the direction perpendicular to that plane.

This process is independent of the coordinate origin, and is determined solely from the local shape of the point cloud.

Looking at FPFH Features in Practice

FPFH (Fast Point Feature Histogram) is a 33-dimensional feature vector.

It represents the "shape around a point" as a histogram.

In other words, points that are:

• On a plane
• On an edge
• On a curved surface

will have different distributions in their FPFH values.

Here, we will visualize the FPFH of a single point as a histogram.

We will also compress all FPFH vectors to 2D (PCA) to see the distribution.

This allows us to confirm that:

Points with similar shapes have similar feature vectors

# Select an arbitrary point
point_idx = 0
feature = fpfh_np[:, point_idx]

plt.figure(figsize=(6,3))
plt.bar(range(33), feature)
plt.title(f"FPFH feature histogram (point {point_idx})")
plt.xlabel("Feature bin")
plt.ylabel("Value")
plt.show()

The figure above shows the FPFH feature of a single point.

The 33 values represent statistics of the angular relationships between normals in the surrounding area of that point.

The normals used here are estimated by fitting a local plane (PCA) to the neighboring points. In other words, we are finding the "surface orientation" from the shape around a point.

FPFH is a histogram of these angular relationships between normals.

Therefore, the shape of this histogram changes depending on:

• Planes
• Edges
• Curved surfaces

and other differences in local shape.

4. Creating Correspondences: FPFH Nearest Neighbor (scikit-learn)

The task is simple:

• For each FPFH (33 dimensions) of source points
• Find the point in target with the most similar FPFH (nearest neighbor)
• Treat that as a "correspondence"

Note: This method results in many incorrect correspondences (outliers). → But that is exactly where TEASER++ shows its strength.

Distribution of FPFH Nearest Neighbor Distances

In the previous step, to create correspondences between two point clouds, we performed nearest neighbor search in FPFH feature space.

Each point has a 33-dimensional FPFH feature vector representing the local shape around it. Here, for each point in the source point cloud, we are finding the point in the target point cloud with the most similar FPFH feature.

The histogram below shows the distribution of distances (L2 distances) between FPFH feature vectors in that process.

from sklearn.neighbors import NearestNeighbors

def fpfh_correspondences(src_down, tgt_down, src_fpfh, tgt_fpfh):
    src_desc = np.asarray(src_fpfh.data).T  # (N, 33)
    tgt_desc = np.asarray(tgt_fpfh.data).T  # (M, 33)

    nn = NearestNeighbors(n_neighbors=1).fit(tgt_desc)
    dists, idx = nn.kneighbors(src_desc, return_distance=True)

    src_pts = np.asarray(src_down.points)
    tgt_pts = np.asarray(tgt_down.points)

    corres_src = src_pts
    corres_tgt = tgt_pts[idx[:, 0]]
    return corres_src, corres_tgt, dists[:, 0]

corres_src, corres_tgt, corres_dist = fpfh_correspondences(
    src_down, tgt_down, src_fpfh, tgt_fpfh
)

print("Number of correspondences:", corres_src.shape[0])
print(
    "FPFH distance min/median/max:",
    np.min(corres_dist),
    np.median(corres_dist),
    np.max(corres_dist),
)

plt.figure()
plt.hist(corres_dist, bins=50)
plt.title("Distribution of FPFH nearest-neighbor distances")
plt.xlabel("L2 distance in FPFH feature space")
plt.ylabel("count")
plt.show()

Number of correspondences: 4760
FPFH distance min/median/max: 0.06863474569887991 22.301490158280416 178.95695674614228

Interpretation:

• Small distance → Local shapes are similar (likely a good correspondence)
• Large distance → Shapes are not very similar (possible false correspondence)

Here, for each point in the source point cloud, we compare it against the FPFH features of all points in the target point cloud, and select the closest point as the correspondence.

In practice, point cloud matching commonly contains many false correspondences (outliers). Therefore, in the next step we use outlier-robust algorithms like TEASER++ to estimate the correct rigid transformation (rotation (R) and translation (t)).

5. Estimating the Rigid Transformation (R,t) with TEASER++

We pass to TEASER++ two sets of corresponding points:

• Source correspondences: {p_i}_i
• Target correspondences: {q_i}_i

TEASER++ estimates (R, t) from these.

When creating correspondences from point cloud features (FPFH), many false correspondences are included.

TEASER++ leverages the property that rigid transformations preserve distances between points:

to find a set of correspondences (inliers) where distance relationships are consistent.

By estimating rotation (R) and translation (t) only from that inlier set, it achieves stable point cloud registration even when many outliers are present.

def run_teaser(corres_src: np.ndarray, corres_tgt: np.ndarray, noise_bound: float):
    # TEASER++ expects a 3xN matrix
    src = corres_src.T.astype(np.float64)
    tgt = corres_tgt.T.astype(np.float64)

    params = teaserpp_python.RobustRegistrationSolver.Params()
    params.noise_bound = float(noise_bound)
    params.cbar2 = 1.0
    params.estimate_scaling = False
    params.rotation_estimation_algorithm = (
        teaserpp_python.RobustRegistrationSolver.ROTATION_ESTIMATION_ALGORITHM.GNC_TLS
    )

    solver = teaserpp_python.RobustRegistrationSolver(params)
    solver.solve(src, tgt)
    sol = solver.getSolution()

    R = np.array(sol.rotation, dtype=np.float64)
    t = np.array(sol.translation, dtype=np.float64).reshape(3)
    return R, t

R_est, t_est = run_teaser(corres_src, corres_tgt, noise_bound=voxel_size * 1.5)

print("Estimated Rotation R:\n", R_est); print('-'*20)
print("Estimated Translation t:\n", t_est)

Starting scale solver (only selecting inliers if scale estimation has been disabled).
Scale estimation complete.
Max core number: 1321
Num vertices: 4761
Max Clique of scale estimation inliers: 
Using chain graph for GNC rotation.
Starting rotation solver.
GNC rotation estimation noise bound:0.15
GNC rotation estimation noise bound squared:0.0225
GNC-TLS solver terminated due to cost convergence.
Cost diff: 0
Iterations: 16
Rotation estimation complete.
Starting translation solver.
Translation estimation complete.
Estimated Rotation R:
 [[ 0.84273474  0.00977796 -0.53824024]
 [-0.15011122  0.96444555 -0.21751185]
 [ 0.51697658  0.26410069  0.81423955]]
--------------------
Estimated Translation t:
 [ 0.62752548  0.81898631 -1.47699519]

6. Applying the Transformation in Open3D and Overlaying the Point Clouds

We create the transformation matrix (T) and apply it to the source.

import copy

def make_T(R, t):
    T = np.eye(4, dtype=np.float64)
    T[:3, :3] = R
    T[:3, 3] = t
    return T

T_est = make_T(R_est, t_est)

src_aligned = copy.deepcopy(src_down)
tgt_vis = copy.deepcopy(tgt_down)

src_aligned.paint_uniform_color([1.0, 0.6, 0.0])  # orange
tgt_vis.paint_uniform_color([0.0, 0.6, 0.9])      # blue

src_aligned.transform(T_est)

o3d.visualization.draw_geometries([src_aligned, tgt_vis], window_name="TEASER++ overlay")

7. Fine Alignment with ICP (Local Refinement)

TEASER++ is "robust to outliers", but the estimated transformation (T=[R|t]) may only be approximately correct (coarse alignment).

This is where ICP (Iterative Closest Point) comes in.

What ICP Does

ICP repeats the following:

1. Use the current transformation (T) to move source closer to target
2. For each point in source, find the "closest point (nearest neighbor)" in target as its correspondence
3. Update the transformation (T) slightly to reduce the error between correspondence pairs
4. Repeat until convergence

The key point is that ICP assumes the point clouds are already close (good initial estimate). A poor initial estimate leads to incorrect nearest neighbor correspondences and convergence to a wrong position.

That's why this Notebook uses the "standard pipeline":

• TEASER++ (Global, outlier-robust) for coarse alignment
• ICP (Local, high precision) for final refinement

Point-to-Point vs Point-to-Plane

There are two main types of ICP:

• point-to-point: Minimizes point-to-point distance (easy to understand)
• point-to-plane: Pushes points toward the other's normal direction (faster convergence, better precision)

In this notebook, we first run point-to-point for clarity, and then try point-to-plane as well.

import copy
import numpy as np
import open3d as o3d

# 0) Estimated transformation from TEASER++ (T_est must already be created)
#    T_est: (4,4) numpy array

# 1) Prepare point clouds for ICP (downsampling speeds things up)
src_icp = copy.deepcopy(src_down)
tgt_icp = copy.deepcopy(tgt_down)

# The threshold (max distance for a valid correspondence) is important for ICP
# Guideline: 1 to 3 times voxel_size
max_corr_dist = voxel_size * 2.0

# 2) Run ICP with TEASER++ result as the initial transformation
result_icp_p2p = o3d.pipelines.registration.registration_icp(
    src_icp,
    tgt_icp,
    max_correspondence_distance=max_corr_dist,
    init=T_est,
    estimation_method=o3d.pipelines.registration.TransformationEstimationPointToPoint(),
)

print("=== ICP (point-to-point) ===")
print("fitness:", result_icp_p2p.fitness)
print("inlier_rmse:", result_icp_p2p.inlier_rmse)
print("T_icp:\n", result_icp_p2p.transformation)

# 3) Visualization: TEASER++ only vs after ICP
src_teaser = copy.deepcopy(src_down)
src_teaser.paint_uniform_color([1.0, 0.6, 0.0])  # orange
tgt_vis = copy.deepcopy(tgt_down)
tgt_vis.paint_uniform_color([0.0, 0.6, 0.9])     # blue

src_teaser.transform(T_est)

src_after_icp = copy.deepcopy(src_down)
src_after_icp.paint_uniform_color([0.2, 1.0, 0.2])  # green
src_after_icp.transform(result_icp_p2p.transformation)

print("Orange = after TEASER++ (coarse)")
print("Green  = after ICP refinement")
print("Blue   = target")
o3d.visualization.draw_geometries([tgt_vis, src_teaser, src_after_icp], window_name="TEASER++ vs ICP")

=== ICP (point-to-point) ===
fitness: 0.6840336134453782
inlier_rmse: 0.025657871153582524
T_icp:
 [[ 0.84142006  0.01381633 -0.54020496  0.62953788]
 [-0.1529137   0.96490184 -0.21349906  0.82072278]
 [ 0.51829499  0.26224713  0.81400046 -1.47781864]
 [ 0.          0.          0.          1.        ]]
Orange = after TEASER++ (coarse)
Green  = after ICP refinement
Blue   = target

Point-to-plane requires normals on the target side. If normals have already been estimated, that's fine. If not, please add estimate_normals below.

# --- Optional: point-to-plane ICP (often better/faster) ---

# Estimate normals if target doesn't have them
if not tgt_icp.has_normals():
    tgt_icp.estimate_normals(
        o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size * 2.0, max_nn=30)
    )

result_icp_p2l = o3d.pipelines.registration.registration_icp(
    src_icp,
    tgt_icp,
    max_correspondence_distance=max_corr_dist,
    init=T_est,
    estimation_method=o3d.pipelines.registration.TransformationEstimationPointToPlane(),
)

print("=== ICP (point-to-plane) ===")
print("fitness:", result_icp_p2l.fitness)
print("inlier_rmse:", result_icp_p2l.inlier_rmse)
print("T_icp_p2l:\n", result_icp_p2l.transformation)

src_after_icp2 = copy.deepcopy(src_down)
src_after_icp2.paint_uniform_color([0.7, 0.2, 1.0])  # purple
src_after_icp2.transform(result_icp_p2l.transformation)

print("Purple = after point-to-plane ICP")
o3d.visualization.draw_geometries([tgt_vis, src_teaser, src_after_icp2], window_name="TEASER++ vs ICP (p2l)")

=== ICP (point-to-plane) ===
fitness: 0.6834033613445378
inlier_rmse: 0.025688621018622015
T_icp_p2l:
 [[ 0.84023928  0.00731303 -0.54216647  0.64994667]
 [-0.14686391  0.96560106 -0.21458235  0.80492176]
 [ 0.52194727  0.2599252   0.81240996 -1.48180895]
 [ 0.          0.          0.          1.        ]]
Purple = after point-to-plane ICP

8. Quantifying Point Cloud Similarity: Overlap / Coverage / Chamfer Distance

After aligning the point clouds, we can numerically evaluate "how similar they are".

Here we compute three representative metrics.

1. Nearest Neighbor Distance

The basic distance used is:

Meaning:

Distance from point a
to the closest point
in point cloud B

Various metrics are built using this distance.

2. Overlap

Overlap represents:

"What proportion of points have a close correspondence"

In other words:

Proportion of points with distance < τ

3. Coverage

Coverage represents:

"How much of point cloud B is covered by A"

The difference from Overlap is just:

which is in the denominator

4. Chamfer Distance

Chamfer Distance represents the:

Average distance between point clouds

Meaning:

Average of nearest neighbor distances in both directions

Commonly used in evaluation of 3D reconstruction and SLAM.

Reference

• AtlasNet: A Papier-Mache Approach to Learning 3D Surface Generation

Let's Compute It

Here we compute:

Overlap
Coverage
Chamfer Distance

using:

• the source after ICP
• the target

import numpy as np
import open3d as o3d

def nn_distances(A, B):
    kdtree = o3d.geometry.KDTreeFlann(B)
    A_pts = np.asarray(A.points)

    dists = np.zeros(len(A_pts))

    for i,p in enumerate(A_pts):
        k, idx, dist2 = kdtree.search_knn_vector_3d(p,1)
        dists[i] = np.sqrt(dist2[0])

    return dists

# Source after ICP
src_eval = src_after_icp2
tgt_eval = tgt_down

# Nearest neighbor distances
d_src_tgt = nn_distances(src_eval, tgt_eval)
d_tgt_src = nn_distances(tgt_eval, src_eval)

# threshold
tau = voxel_size * 2

# Overlap
overlap = np.mean(d_src_tgt < tau)

# Coverage
coverage = np.mean(d_tgt_src < tau)

# Chamfer Distance
chamfer = np.mean(d_src_tgt**2) + np.mean(d_tgt_src**2)

print("Overlap:", overlap)
print("Coverage:", coverage)
print("Chamfer Distance:", chamfer)

Overlap: 0.6834033613445378
Coverage: 0.977906976744186
Chamfer Distance: 0.15243212483783478

Looking at the distance distribution, we can understand "how many points are misaligned and by how much".

import matplotlib.pyplot as plt

plt.figure(figsize=(6,4))

plt.hist(d_src_tgt, bins=50, alpha=0.6, label="source → target")
plt.hist(d_tgt_src, bins=50, alpha=0.6, label="target → source")

plt.axvline(tau, linestyle="--", color="black")

plt.legend()
plt.xlabel("Nearest Neighbor Distance")
plt.ylabel("count")
plt.title("Distance distribution between point clouds")

plt.show()

The distributions of source→target and target→source distances differ because nearest neighbor distance is asymmetric.

source→target represents: "How close each point in source is to the target"

target→source represents: "How close each point in target is to the source"

When point cloud density or coverage differs, these two distance distributions generally have different shapes.

Summary

In this Notebook, we experienced the Point Cloud Registration Pipeline for aligning two point clouds.

Processing flow:

Point Clouds
 ↓
Voxel Downsampling
 ↓
Normal Estimation
 ↓
FPFH Features
 ↓
Feature Matching
 ↓
TEASER++ (outlier-robust rigid transformation estimation)
 ↓
ICP (local precise alignment)
 ↓
Similarity evaluation with Overlap / Coverage / Chamfer

Summary of roles:

This pipeline is the foundational technology for many 3D processing tasks such as:

• SLAM
• 3D scanning
• Digital twins