Build a 3D Scene Reconstruction Pipeline

Reconstruct a 3D mesh from RGB-D frames using FPFH, ICP, and TSDF fusion.

Introduction

30 Second Summary

Think about how your phone can scan a room in 3D, or how self-driving cars build maps of their surroundings. These systems reconstruct the physical world from camera images, turning flat pixels into navigable 3D geometry.

In this project, you will build a complete multi-view 3D scene reconstruction pipeline using Open3D. Starting from raw RGB-D image pairs (color + depth), you'll compute camera motion, align point clouds using feature-based registration, and fuse everything into a volumetric representation that outputs a final 3D triangle mesh.

What You'll Build

A Python pipeline that takes RGB-D frames and produces a reconstructed 3D triangle mesh of an indoor scene, using the same algorithmic building blocks found in production SLAM and 3D scanning systems.

By the end of this project, you'll have:

  • A working RGBD odometry system that computes frame-to-frame camera motion using hybrid photometric and geometric constraints.
  • A global registration pipeline using FPFH feature descriptors and RANSAC matching, refined by point-to-plane ICP for sub-centimeter accuracy.
  • A TSDF volumetric integration system that fuses multiple depth frames into a smooth, noise-reduced 3D mesh extracted via marching cubes.
  • Secret Mission: Implement multi-scale Colored ICP registration that fuses geometric and photometric constraints for tighter alignment on textured surfaces.

Are there any prerequisites?

This project assumes familiarity with Python and a basic understanding of linear algebra (transformation matrices, eigendecomposition). You should also be comfortable with 3D coordinate systems.

No prior experience with 3D vision libraries or point cloud processing is required. The project introduces each algorithm step by step.

Before We Start

3D reconstruction is how machines understand physical spaces. It's the core technology behind robotic mapping, autonomous navigation, AR/VR scene capture, and 3D scanning.

In this step, you'll install Open3D (a purpose-built library for 3D data processing), create your project file, and verify that the built-in sample dataset downloads correctly.

In this step, get ready to:
  • Install Open3D and its dependencies.
  • Create your project file with the required imports.
  • Verify access to the built-in Redwood RGBD sample dataset.
Install Open3D and dependencies

Open3D is a Python library designed specifically for 3D data processing. It provides algorithms for point cloud manipulation, registration, and volumetric integration that you'll use throughout this pipeline.

  • Open your terminal by pressing Cmd+Space (macOS) or the Windows key (Windows), typing Terminal (macOS) or PowerShell (Windows), and pressing Enter.
  • Confirm your Python version is 3.10 or higher by running this command:
python3 --version

Seeing a version lower than 3.10?

Open3D 0.19.0 requires Python 3.10 or higher. If your version is lower, download the latest Python from python.org and install it before continuing.

On macOS, the system Python may be older than what you installed. Try running python3.10 --version or python3.11 --version to check if a newer version is available under a versioned name.

help me figure out which Python version I have and how to get 3.10+

  • Move to your Desktop by running this command:
cd ~/Desktop
  • Create your project folder and move into it by running these commands:
mkdir 3d-reconstruction
cd 3d-reconstruction
  • Install Open3D and the other dependencies by running:
pip install open3d==0.19.0 numpy matplotlib

What does this command install?

  • open3d==0.19.0 is the 3D data processing library pinned to the exact version this project uses.
  • numpy provides the matrix math and array operations that power every transformation in the pipeline.
  • matplotlib handles 2D image visualization for inspecting color and depth frames.
  • Verify the installation succeeded by running:
python -c "import open3d as o3d; print(o3d.__version__)"
  • You should see 0.19.0 printed in your terminal.

Seeing an import error or wrong version?

  • If you see ModuleNotFoundError, make sure you ran the pip install command in the same terminal session and that pip points to your Python 3.10+ installation.
  • Try using pip3 instead of pip if your system has both Python 2 and Python 3.
  • On Windows, you may need to use py -m pip install instead.

help me fix my Open3D installation

Create your project file

Your entire reconstruction pipeline will live in a single Python file. This file will grow step by step as you add each stage of the pipeline.

  • Create the file by running:
touch reconstruction_pipeline.py
  • Confirm the file exists by running:
ls
  • You should see reconstruction_pipeline.py in the output.
  • Open reconstruction_pipeline.py in your preferred code editor and add the following imports:
import open3d as o3d
import numpy as np
import copy
import matplotlib.pyplot as plt

What do these imports do?

  • open3d (aliased as o3d) provides all the 3D processing algorithms: point clouds, registration, and volumetric integration.
  • numpy (aliased as np) handles transformation matrices and array operations.
  • copy lets you deep-copy point clouds for visualization without modifying the originals.
  • matplotlib.pyplot (aliased as plt) displays 2D color and depth images.
  • Save reconstruction_pipeline.py.

✔️ Awesome, I've got everything!

Great. Double check you've saved your file before moving on.

ⓧ I'd like to double check the full code

import open3d as o3d
import numpy as np
import copy
import matplotlib.pyplot as plt
Verify the Redwood RGBD sample dataset

This project uses Open3D's built-in Redwood RGBD dataset, which contains 5 synchronized color and depth image pairs with ground-truth camera poses. Open3D auto-downloads it (~25MB) the first time you access it.

  • Verify that the dataset downloads and loads correctly by running:
python -c "import open3d as o3d; data = o3d.data.SampleRedwoodRGBDImages(); print(f'Color images: {len(data.color_paths)}'); print(f'Depth images: {len(data.depth_paths)}')"
  • You should see this output:
Color images: 5
Depth images: 5

What is the Redwood RGBD dataset?

It contains 5 frames of an indoor scene captured with an RGBD camera (a sensor that records both color and depth per pixel). The depth images are 16-bit, storing distance in millimeters.

The dataset also includes an odometry log file with ground-truth camera poses (4x4 transformation matrices) that tell you exactly where the camera was for each frame.

Download failing or timing out?

  • The dataset downloads from GitHub on first run. If your network blocks GitHub, try on a different connection.
  • If you see a permissions error, make sure you have write access to your home directory where Open3D caches data.

help me troubleshoot the Open3D dataset download

Your environment is ready and the dataset is cached locally. Next up, you'll convert those raw RGB-D frames into 3D point clouds using camera intrinsics.

Load RGBD Data and Create Point Clouds

Your environment is ready with Open3D installed and your project file created. Now it's time to bring in real 3D data.

3D reconstruction starts with RGB-D images. These are pairs of a regular color photo and a depth map that records how far away each pixel is from the camera. In this step, you will load these image pairs and convert them into 3D point clouds that you can visualize and manipulate.

In this step, get ready to:
  • Read color and depth images and combine them into an RGBD image.
  • Convert an RGBD image into a 3D point cloud using camera intrinsics.
  • Load a camera trajectory that provides each frame's position in 3D space.
Read color and depth images

Before we load images, the pipeline needs a few utility functions that will be used throughout the project. These handle camera pose storage, trajectory file parsing, and registration visualization.

  • In reconstruction_pipeline.py, find this line:
import matplotlib.pyplot as plt
  • Delete this line. The pipeline uses Open3D's built-in visualizer instead of Matplotlib, so this import is no longer needed.
  • Below import copy, add the following utility functions:

class CameraPose:
    def __init__(self, meta, mat):
        self.metadata = meta
        self.pose = mat

    def __str__(self):
        return 'Metadata : ' + ' '.join(map(str, self.metadata)) + '\n' + \
            "Pose : " + "\n" + np.array_str(self.pose)


def read_trajectory(filename):
    """Parse an odometry .log file into a list of CameraPose objects."""
    traj = []
    with open(filename, 'r') as f:
        metastr = f.readline()
        while metastr:
            metadata = list(map(int, metastr.split()))
            mat = np.zeros(shape=(4, 4))
            for i in range(4):
                matstr = f.readline()
                mat[i, :] = np.fromstring(matstr, dtype=float, sep=' \t')
            traj.append(CameraPose(metadata, mat))
            metastr = f.readline()
    return traj


def draw_registration_result(source, target, transformation):
    """Visualize two point clouds after applying a transformation to source."""
    source_temp = copy.deepcopy(source)
    target_temp = copy.deepcopy(target)
    source_temp.paint_uniform_color([1, 0.706, 0])
    target_temp.paint_uniform_color([0, 0.651, 0.929])
    source_temp.transform(transformation)
    o3d.visualization.draw_geometries([source_temp, target_temp])

What do these utility functions do?

CameraPose stores a 4x4 transformation matrix representing a camera's position and orientation in 3D space.

read_trajectory() parses an odometry log file where each entry has 3 integers of metadata followed by a 4x4 matrix. It returns a list of CameraPose objects.

draw_registration_result() colors two point clouds orange and blue, applies a transformation to the source, and renders them side by side for visual comparison.

  • You won't see output from these yet. They are used in later steps when you align point clouds and load camera poses.

Now for the core task of this substep. An RGBD image combines a regular color photo with a depth map. The depth map records how far each pixel is from the camera as a 16-bit integer value in millimeters.

  • At the bottom of reconstruction_pipeline.py, add the following code to load the dataset, read the first color and depth images, and create an RGBD image:
print("=" * 60)
print("STEP 2: Loading RGBD Data and Creating Point Clouds")
print("=" * 60)

# Load built-in Redwood RGBD dataset (auto-downloads on first run)
redwood_rgbd = o3d.data.SampleRedwoodRGBDImages()

# Read first color and depth image
color_raw = o3d.io.read_image(redwood_rgbd.color_paths[0])
depth_raw = o3d.io.read_image(redwood_rgbd.depth_paths[0])

# Create RGBD image (depth in mm -> converted to meters internally)
rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
    color_raw, depth_raw)
print(f"RGBD Image: {rgbd_image}")

What does this code do?

  • o3d.data.SampleRedwoodRGBDImages() downloads the Redwood RGBD dataset (~25MB on first run). It contains 5 synchronized color and depth frame pairs with ground-truth camera poses.
  • o3d.io.read_image() loads image files into Open3D image objects.
  • create_from_color_and_depth() fuses them into a single RGBD image. It converts the 16-bit depth (in millimeters) to float meters and normalizes color to [0, 1].
  • Save reconstruction_pipeline.py and run it in your terminal:
python reconstruction_pipeline.py
  • You should see the dataset download (first run only), followed by RGBD Image: with details about the color and depth image dimensions printed in your terminal.

Seeing a download error?

The dataset downloads from GitHub on first run. Make sure you have an active internet connection. If you see a timeout, try running the script again.

If you see a NameError on np or o3d, confirm your imports at the top of the file match import open3d as o3d and import numpy as np.

Help me debug my RGBD image loading error in Open3D

Convert the RGBD image to a point cloud

An RGBD image alone is still a flat 2D representation. To get actual 3D coordinates, you need camera intrinsics. These parameters describe the camera's focal length and optical center, which let you project each pixel into 3D space.

  • Add the following code at the bottom of reconstruction_pipeline.py to create a point cloud from the RGBD image and visualize it:
# Convert to point cloud using PrimeSense camera intrinsics
# fx=fy=525.0, cx=319.5, cy=239.5, resolution 640x480
intrinsic = o3d.camera.PinholeCameraIntrinsic(
    o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault)
print(f"Camera intrinsic matrix:\n{intrinsic.intrinsic_matrix}")

pcd = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd_image, intrinsic)
# Flip for visualization (camera looks along -Z)
pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])
print(f"Point cloud has {len(pcd.points)} points")
o3d.visualization.draw_geometries([pcd])

What does this code do?

  • PinholeCameraIntrinsicParameters.PrimeSenseDefault provides pre-set intrinsics for the PrimeSense depth sensor: focal lengths fx=fy=525.0, optical center cx=319.5, cy=239.5, at 640x480 resolution.
  • create_from_rgbd_image() back-projects each pixel into 3D using the depth value and intrinsics. Each pixel becomes a 3D point with (x, y, z) coordinates and a color.
  • The flip transformation negates the Y and Z axes. This converts from the camera coordinate system (Z pointing into the scene) to a visualization-friendly frame.
  • Save reconstruction_pipeline.py and run it again:
python reconstruction_pipeline.py
  • You should see a 3x3 intrinsic matrix printed, followed by a point count (several hundred thousand points). A 3D visualization window will open showing a colored point cloud of an indoor scene.
  • Close the visualization window to let the script continue.

Visualization window not appearing?

Open3D's visualizer requires a display. If you are running on a headless server (no monitor), the processing will still work but the window cannot render. Run on a machine with a display to see the point cloud.

If the window opens but appears black, try clicking and dragging to rotate the view. The point cloud may be oriented away from the default camera angle.

Help me debug my Open3D point cloud visualization

Load the camera trajectory

Each frame in the dataset was captured from a different camera position. The odometry log file records these positions as 4x4 transformation matrices. You need them to place each frame's point cloud in a shared coordinate system.

  • Add this code at the bottom of reconstruction_pipeline.py to load all 5 camera poses from the odometry log:
# Load camera trajectory from odometry log
camera_poses = read_trajectory(redwood_rgbd.odometry_log_path)
print(f"Loaded {len(camera_poses)} camera poses")

What does this code do?

redwood_rgbd.odometry_log_path points to a .log file bundled with the dataset. Each entry contains 3 metadata integers and a 4x4 matrix encoding the camera's position and rotation for that frame.

The read_trajectory() function you added earlier parses this file and returns a list of 5 CameraPose objects. Each object's .pose field is a 4x4 matrix you will use later to fuse all frames together.

  • Save reconstruction_pipeline.py and run it one more time:
python reconstruction_pipeline.py
  • After the visualization window closes, you should see Loaded 5 camera poses printed in your terminal. This confirms all 5 frame poses were parsed correctly.

Seeing a file not found error?

If read_trajectory() throws a FileNotFoundError, the dataset may not have downloaded fully. Delete any partial download cache and re-run the script to trigger a fresh download.

If you see a ValueError from np.fromstring, make sure you copied the read_trajectory() function exactly, including the separator string ' \t' (space followed by tab).

Help me debug my camera trajectory loading error

✔️ Awesome, I've got everything!

Great. Double check you have saved reconstruction_pipeline.py before moving on.

ⓧ I'd like to double check the full code

import open3d as o3d
import numpy as np
import copy


class CameraPose:
    def __init__(self, meta, mat):
        self.metadata = meta
        self.pose = mat

    def __str__(self):
        return 'Metadata : ' + ' '.join(map(str, self.metadata)) + '\n' + \
            "Pose : " + "\n" + np.array_str(self.pose)


def read_trajectory(filename):
    traj = []
    with open(filename, 'r') as f:
        metastr = f.readline()
        while metastr:
            metadata = list(map(int, metastr.split()))
            mat = np.zeros(shape=(4, 4))
            for i in range(4):
                matstr = f.readline()
                mat[i, :] = np.fromstring(matstr, dtype=float, sep=' \t')
            traj.append(CameraPose(metadata, mat))
            metastr = f.readline()
    return traj


def draw_registration_result(source, target, transformation):
    source_temp = copy.deepcopy(source)
    target_temp = copy.deepcopy(target)
    source_temp.paint_uniform_color([1, 0.706, 0])
    target_temp.paint_uniform_color([0, 0.651, 0.929])
    source_temp.transform(transformation)
    o3d.visualization.draw_geometries([source_temp, target_temp])


print("=" * 60)
print("STEP 2: Loading RGBD Data and Creating Point Clouds")
print("=" * 60)

redwood_rgbd = o3d.data.SampleRedwoodRGBDImages()

color_raw = o3d.io.read_image(redwood_rgbd.color_paths[0])
depth_raw = o3d.io.read_image(redwood_rgbd.depth_paths[0])

rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
    color_raw, depth_raw)
print(f"RGBD Image: {rgbd_image}")

intrinsic = o3d.camera.PinholeCameraIntrinsic(
    o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault)
print(f"Camera intrinsic matrix:\n{intrinsic.intrinsic_matrix}")

pcd = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd_image, intrinsic)
pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])
print(f"Point cloud has {len(pcd.points)} points")
o3d.visualization.draw_geometries([pcd])

camera_poses = read_trajectory(redwood_rgbd.odometry_log_path)
print(f"Loaded {len(camera_poses)} camera poses")

You now have 3D point clouds and camera poses loaded. Next up, you will compute the relative motion between consecutive frames using RGBD odometry.

Compute RGBD Odometry

You now have a single point cloud from one RGB-D frame and a set of known camera poses. The next challenge is figuring out how the camera moved between consecutive frames using the images themselves.

This process is called RGBD odometry. It estimates the rigid-body transformation (rotation + translation) between two frames by combining photometric consistency (color similarity) with geometric constraints (depth agreement).

In this step, get ready to:
  • Load two consecutive RGB-D frames as source and target.
  • Compute the camera motion between frames using hybrid odometry.
  • Visualize the aligned point clouds to verify the transformation.
Load two consecutive RGBD frames

Odometry requires a pair of frames to compare. You need a "source" (frame 0) and a "target" (frame 1) so the algorithm can determine how the camera moved between them.

  • In reconstruction_pipeline.py, add the following code at the bottom of the file (after the line that prints loaded camera poses):
print("\n" + "=" * 60)
print("STEP 3: Computing RGBD Odometry")
print("=" * 60)

# Read two consecutive RGBD frames
source_color = o3d.io.read_image(redwood_rgbd.color_paths[0])
source_depth = o3d.io.read_image(redwood_rgbd.depth_paths[0])
target_color = o3d.io.read_image(redwood_rgbd.color_paths[1])
target_depth = o3d.io.read_image(redwood_rgbd.depth_paths[1])

source_rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(
    source_color, source_depth)
target_rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(
    target_color, target_depth)

What does this code do?

You load color and depth images for frame 0 (source) and frame 1 (target) from the Redwood dataset.

Each pair is combined into an RGBDImage object using create_from_color_and_depth(). This converts 16-bit depth (millimeters) into meters and normalizes color to a float range.

  • Save reconstruction_pipeline.py.

You won't see a change yet. The next chunk uses these RGBD images to compute the actual transformation.

Compute the odometry transformation

With both frames loaded, you can now run the odometry algorithm. The hybrid Jacobian method (from Park et al., 2017) jointly minimizes photometric error and geometric distance for robust motion estimation.

  • Add the following code below the target_rgbd line you just wrote:
# Compute odometry using hybrid term (Park2017: photo + geometric)
option = o3d.pipelines.odometry.OdometryOption()
print(f"Odometry options: {option}")

odo_init = np.identity(4)
[success, trans, info] = o3d.pipelines.odometry.compute_rgbd_odometry(
    source_rgbd, target_rgbd, intrinsic, odo_init,
    o3d.pipelines.odometry.RGBDOdometryJacobianFromHybridTerm(), option)

What does this code do?

  • OdometryOption() configures the algorithm with default thresholds: depth_diff_max=0.03 (max depth difference for correspondence matching), depth_min=0 and depth_max=4 (valid depth range in meters).
  • odo_init = np.identity(4) provides the initial guess for the transformation. An identity matrix means "assume no motion" as a starting point.
  • RGBDOdometryJacobianFromHybridTerm() combines photometric consistency (matching pixel colors) with geometric constraints (matching depth surfaces). This is more robust than using either constraint alone.
  • The function returns three values: a success flag, the 4x4 transformation matrix, and an information matrix describing estimation confidence.

Why hybrid over color-only odometry?

A color-only approach struggles in regions with little texture (blank walls, flat surfaces). A geometry-only approach fails on planar surfaces where depth alone cannot disambiguate lateral motion. The hybrid term handles both scenarios by fusing both signals into a single optimization.

  • Save reconstruction_pipeline.py.
  • Run the script with:
python reconstruction_pipeline.py

After the Step 2 visualization window (close it to continue), you should see the odometry options printed and then Odometry successful! followed by a 4x4 transformation matrix.

Not seeing odometry output?

  • Close the Open3D visualization window from Step 2. The script pauses at each draw_geometries() call until you close the window.
  • Make sure you placed the new code after the last line from the previous step (print(f"Loaded {len(camera_poses)} camera poses")).

Why isn't my odometry computation running?

Visualize the aligned point clouds

The transformation matrix tells you how the camera moved, but the real test is visual. By transforming the source point cloud with the estimated motion, both frames should overlap tightly in 3D space.

  • Add the following code below the compute_rgbd_odometry() call:
if success:
    print("Odometry successful!")
    print(f"Transformation:\n{trans}")

    # Visualize aligned point clouds
    target_pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
        target_rgbd, intrinsic)
    source_pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
        source_rgbd, intrinsic)
    source_pcd.transform(trans)
    o3d.visualization.draw_geometries([target_pcd, source_pcd])

What does this code do?

  • The if success: guard ensures you only proceed when the odometry algorithm converged to a valid solution.
  • Both RGBD frames are converted into point clouds using the same PrimeSenseDefault camera intrinsics from earlier.
  • source_pcd.transform(trans) applies the estimated 4x4 transformation to move the source into the target's coordinate frame. If odometry is accurate, the two clouds overlap.
  • Save reconstruction_pipeline.py and run the script again:
python reconstruction_pipeline.py

Close the Step 2 visualization window. You should then see the transformation matrix printed, followed by a new visualization window showing both point clouds overlapping tightly.

Point clouds look misaligned?

  • Make sure the if success: block is indented correctly. All the code inside it (the print statements, point cloud creation, transform, and visualization) must be indented one level under the if.
  • Verify you are passing trans (the odometry result) to source_pcd.transform(), not odo_init.

Help me debug why my odometry-aligned point clouds don't overlap.

✔️ Awesome, I've got everything!

Great. Double check you've saved your file before moving on.

ⓧ I'd like to double check the full code

Here is the full reconstruction_pipeline.py up to this point:

import open3d as o3d
import numpy as np
import copy


class CameraPose:
    def __init__(self, meta, mat):
        self.metadata = meta
        self.pose = mat

    def __str__(self):
        return 'Metadata : ' + ' '.join(map(str, self.metadata)) + '\n' + \
            "Pose : " + "\n" + np.array_str(self.pose)


def read_trajectory(filename):
    traj = []
    with open(filename, 'r') as f:
        metastr = f.readline()
        while metastr:
            metadata = list(map(int, metastr.split()))
            mat = np.zeros(shape=(4, 4))
            for i in range(4):
                matstr = f.readline()
                mat[i, :] = np.fromstring(matstr, dtype=float, sep=' \t')
            traj.append(CameraPose(metadata, mat))
            metastr = f.readline()
    return traj


def draw_registration_result(source, target, transformation):
    source_temp = copy.deepcopy(source)
    target_temp = copy.deepcopy(target)
    source_temp.paint_uniform_color([1, 0.706, 0])
    target_temp.paint_uniform_color([0, 0.651, 0.929])
    source_temp.transform(transformation)
    o3d.visualization.draw_geometries([source_temp, target_temp])


print("=" * 60)
print("STEP 2: Loading RGBD Data and Creating Point Clouds")
print("=" * 60)

redwood_rgbd = o3d.data.SampleRedwoodRGBDImages()

color_raw = o3d.io.read_image(redwood_rgbd.color_paths[0])
depth_raw = o3d.io.read_image(redwood_rgbd.depth_paths[0])

rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
    color_raw, depth_raw)
print(f"RGBD Image: {rgbd_image}")

intrinsic = o3d.camera.PinholeCameraIntrinsic(
    o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault)
print(f"Camera intrinsic matrix:\n{intrinsic.intrinsic_matrix}")

pcd = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd_image, intrinsic)
pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])
print(f"Point cloud has {len(pcd.points)} points")
o3d.visualization.draw_geometries([pcd])

camera_poses = read_trajectory(redwood_rgbd.odometry_log_path)
print(f"Loaded {len(camera_poses)} camera poses")

print("\n" + "=" * 60)
print("STEP 3: Computing RGBD Odometry")
print("=" * 60)

source_color = o3d.io.read_image(redwood_rgbd.color_paths[0])
source_depth = o3d.io.read_image(redwood_rgbd.depth_paths[0])
target_color = o3d.io.read_image(redwood_rgbd.color_paths[1])
target_depth = o3d.io.read_image(redwood_rgbd.depth_paths[1])

source_rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(
    source_color, source_depth)
target_rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(
    target_color, target_depth)

option = o3d.pipelines.odometry.OdometryOption()
print(f"Odometry options: {option}")

odo_init = np.identity(4)
[success, trans, info] = o3d.pipelines.odometry.compute_rgbd_odometry(
    source_rgbd, target_rgbd, intrinsic, odo_init,
    o3d.pipelines.odometry.RGBDOdometryJacobianFromHybridTerm(), option)

if success:
    print("Odometry successful!")
    print(f"Transformation:\n{trans}")

    target_pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
        target_rgbd, intrinsic)
    source_pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
        source_rgbd, intrinsic)
    source_pcd.transform(trans)
    o3d.visualization.draw_geometries([target_pcd, source_pcd])

You now have frame-to-frame motion estimation working. Next up, you'll tackle a harder problem: aligning two point clouds that have no initial pose guess at all, using geometric feature descriptors and RANSAC.

Global Registration with FPFH Features

In the previous step, you computed frame-to-frame motion using RGBD odometry. That works well for consecutive frames, but it relies on having a good initial guess of how the camera moved.

What if two point clouds come from completely different viewpoints with no overlap assumption? This is where global registration comes in. You will use FPFH (Fast Point Feature Histograms) to describe the geometry around each point, then let RANSAC find the transformation that aligns the most feature matches.

In this step, get ready to:
  • Build a preprocessing pipeline that downsamples point clouds and computes FPFH feature descriptors.
  • Execute RANSAC-based global registration to align point clouds without an initial pose.
  • Evaluate registration quality using fitness and inlier RMSE metrics.
Implement the point cloud preprocessing function

Before you can match features between two point clouds, each cloud needs three things: fewer points (downsampled for speed), surface normals (for local geometry), and FPFH descriptors (a 33-dimensional fingerprint of the local shape around each point).

  • In reconstruction_pipeline.py, scroll to the bottom of the file (after the Step 3 odometry code) and add the following section header, voxel size, and preprocessing function:
print("\n" + "=" * 60)
print("STEP 4: Global Registration (FPFH + RANSAC)")
print("=" * 60)

voxel_size = 0.05


def preprocess_point_cloud(pcd, voxel_size):
    """Downsample, estimate normals, compute FPFH features."""
    print(f":: Downsample with voxel size {voxel_size:.3f}")
    pcd_down = pcd.voxel_down_sample(voxel_size)

    radius_normal = voxel_size * 2
    print(f":: Estimate normals with search radius {radius_normal:.3f}")
    pcd_down.estimate_normals(
        o3d.geometry.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30))

    radius_feature = voxel_size * 5
    print(f":: Compute FPFH features with search radius {radius_feature:.3f}")
    pcd_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
        pcd_down,
        o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100))
    return pcd_down, pcd_fpfh

What does this code do?

The voxel_size = 0.05 sets 5cm voxels for downsampling. This controls the density of the processed cloud.

Inside preprocess_point_cloud(), three operations happen in sequence:

  • voxel_down_sample replaces all points within each 5cm cube with a single averaged point, reducing the cloud size dramatically.
  • estimate_normals computes a surface normal for each point using its 30 nearest neighbours within a radius of voxel_size * 2 (0.1m).
  • compute_fpfh_feature builds a 33-dimensional histogram descriptor for each point, encoding the angular relationships between its normal and its neighbours' normals within voxel_size * 5 (0.25m).

What is FPFH?

FPFH (Fast Point Feature Histograms) encodes the shape of the surface around each point into a compact descriptor. Two points on similar surfaces (like a flat wall or a curved corner) will have similar FPFH vectors, even if the point clouds are in completely different coordinate frames.

This is what makes pose-free alignment possible. RANSAC can sample pairs of points with matching FPFH descriptors and check whether they agree on a single rigid transformation.

Now you need point clouds to register. For this step, you will switch to the DemoICPPointClouds dataset, which provides pre-processed point clouds ideal for demonstrating registration algorithms.

  • Add the following code directly below the preprocess_point_cloud() function to load the demo dataset, apply a misalignment, and preprocess both clouds:
# Load two point clouds from the DemoICP dataset
demo_icp_pcds = o3d.data.DemoICPPointClouds()
source = o3d.io.read_point_cloud(demo_icp_pcds.paths[0])
target = o3d.io.read_point_cloud(demo_icp_pcds.paths[1])

# Apply an arbitrary misalignment to source
trans_init = np.asarray([[0.0, 0.0, 1.0, 0.0],
                         [1.0, 0.0, 0.0, 0.0],
                         [0.0, 1.0, 0.0, 0.0],
                         [0.0, 0.0, 0.0, 1.0]])
source.transform(trans_init)

# Preprocess both point clouds
source_down, source_fpfh = preprocess_point_cloud(source, voxel_size)
target_down, target_fpfh = preprocess_point_cloud(target, voxel_size)

Why apply a misalignment?

The trans_init matrix is a rotation that scrambles the source cloud's orientation. This simulates the real-world scenario where two scans have no shared coordinate frame. Global registration must recover the correct alignment purely from geometric features.

  • Save reconstruction_pipeline.py.

Seeing an import error?

If you see an error about pipelines.registration not being found, make sure you installed open3d==0.19.0 in Step 1. Older versions use a different module path for registration functions.

Help me debug an Open3D registration import error.

Execute RANSAC-based global registration

With FPFH descriptors computed for both clouds, RANSAC can now sample random feature correspondences, propose transformations, and keep the one with the most inlier agreements.

  • Add the RANSAC registration call below the preprocessing code:
# Execute RANSAC global registration
distance_threshold = voxel_size * 1.5
print(f":: RANSAC registration with distance threshold {distance_threshold:.3f}")

result_ransac = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
    source_down, target_down, source_fpfh, target_fpfh, True,
    distance_threshold,
    o3d.pipelines.registration.TransformationEstimationPointToPoint(False),
    3, [
        o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9),
        o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(
            distance_threshold)
    ], o3d.pipelines.registration.RANSACConvergenceCriteria(100000, 0.999))

What does this code do?

This is a single function call with several important parameters:

  • distance_threshold = voxel_size * 1.5 (0.075m) defines how close a correspondence must be to count as an inlier.
  • CorrespondenceCheckerBasedOnEdgeLength(0.9) rejects correspondences where the relative edge lengths differ by more than 10%. This prunes geometrically inconsistent matches.
  • CorrespondenceCheckerBasedOnDistance rejects correspondences exceeding the distance threshold after applying the proposed transformation.
  • RANSACConvergenceCriteria(100000, 0.999) runs up to 100,000 iterations or until 99.9% confidence is reached.
  • Save reconstruction_pipeline.py.

Script taking a very long time?

RANSAC with 100,000 iterations on downsampled data typically finishes in a few seconds. If it hangs, check that you are passing the downsampled clouds (source_down, target_down) and not the full-resolution originals.

Help me debug slow RANSAC registration in Open3D.

Evaluate registration quality and visualize

RANSAC returns a registration result object with two key metrics: fitness (what fraction of target points have a close correspondence) and inlier_rmse (root mean squared error of those correspondences).

  • Add the result printing and visualization below the RANSAC call:
print(f"RANSAC Result:")
print(f"  Fitness: {result_ransac.fitness:.6f}")
print(f"  Inlier RMSE: {result_ransac.inlier_rmse:.6f}")
print(f"  Correspondences: {len(result_ransac.correspondence_set)}")
draw_registration_result(source_down, target_down, result_ransac.transformation)

What do these metrics mean?

  • fitness is the number of inlier correspondences divided by the number of points in the target. A value near 0.67 means about two-thirds of the target surface has a matching source point within the distance threshold.
  • inlier_rmse measures how tightly the inliers align. Lower is better. RANSAC typically achieves an RMSE around 0.037 on this dataset, which is rough but sufficient as a starting point for ICP refinement.
  • Save reconstruction_pipeline.py and run the full script:
python reconstruction_pipeline.py
  • Close each visualization window as it appears (Steps 2 and 3 will show first). When the final RANSAC visualization opens, you should see the source cloud (orange) roughly aligned with the target cloud (blue). The overlap is visible but not pixel-perfect.

Why is the alignment only approximate?

RANSAC operates on heavily downsampled data (5cm voxels) and uses random sampling. It finds the correct general orientation but lacks the precision of iterative methods. This is by design. The next step uses ICP to refine this rough alignment down to sub-centimetre accuracy.

Clouds look completely misaligned?

If the orange and blue clouds don't overlap at all, check that you applied source.transform(trans_init) before calling preprocess_point_cloud(). The misalignment must happen before feature extraction so RANSAC has a real problem to solve.

Also verify you are passing source_fpfh and target_fpfh (the FPFH features) to the RANSAC call, not the point clouds themselves.

Help me debug my RANSAC global registration result.

✔️ Awesome, I've got everything!

Great. Double check you have saved reconstruction_pipeline.py with the full Step 4 code.

ⓧ I'd like to double check the full code

import open3d as o3d
import numpy as np
import copy


class CameraPose:
    def __init__(self, meta, mat):
        self.metadata = meta
        self.pose = mat

    def __str__(self):
        return 'Metadata : ' + ' '.join(map(str, self.metadata)) + '\n' + \
            "Pose : " + "\n" + np.array_str(self.pose)


def read_trajectory(filename):
    traj = []
    with open(filename, 'r') as f:
        metastr = f.readline()
        while metastr:
            metadata = list(map(int, metastr.split()))
            mat = np.zeros(shape=(4, 4))
            for i in range(4):
                matstr = f.readline()
                mat[i, :] = np.fromstring(matstr, dtype=float, sep=' \t')
            traj.append(CameraPose(metadata, mat))
            metastr = f.readline()
    return traj


def draw_registration_result(source, target, transformation):
    source_temp = copy.deepcopy(source)
    target_temp = copy.deepcopy(target)
    source_temp.paint_uniform_color([1, 0.706, 0])
    target_temp.paint_uniform_color([0, 0.651, 0.929])
    source_temp.transform(transformation)
    o3d.visualization.draw_geometries([source_temp, target_temp])


print("=" * 60)
print("STEP 2: Loading RGBD Data and Creating Point Clouds")
print("=" * 60)

redwood_rgbd = o3d.data.SampleRedwoodRGBDImages()

color_raw = o3d.io.read_image(redwood_rgbd.color_paths[0])
depth_raw = o3d.io.read_image(redwood_rgbd.depth_paths[0])

rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
    color_raw, depth_raw)
print(f"RGBD Image: {rgbd_image}")

intrinsic = o3d.camera.PinholeCameraIntrinsic(
    o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault)
print(f"Camera intrinsic matrix:\n{intrinsic.intrinsic_matrix}")

pcd = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd_image, intrinsic)
pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])
print(f"Point cloud has {len(pcd.points)} points")
o3d.visualization.draw_geometries([pcd])

camera_poses = read_trajectory(redwood_rgbd.odometry_log_path)
print(f"Loaded {len(camera_poses)} camera poses")

print("\n" + "=" * 60)
print("STEP 3: Computing RGBD Odometry")
print("=" * 60)

source_color = o3d.io.read_image(redwood_rgbd.color_paths[0])
source_depth = o3d.io.read_image(redwood_rgbd.depth_paths[0])
target_color = o3d.io.read_image(redwood_rgbd.color_paths[1])
target_depth = o3d.io.read_image(redwood_rgbd.depth_paths[1])

source_rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(
    source_color, source_depth)
target_rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(
    target_color, target_depth)

option = o3d.pipelines.odometry.OdometryOption()
print(f"Odometry options: {option}")

odo_init = np.identity(4)
[success, trans, info] = o3d.pipelines.odometry.compute_rgbd_odometry(
    source_rgbd, target_rgbd, intrinsic, odo_init,
    o3d.pipelines.odometry.RGBDOdometryJacobianFromHybridTerm(), option)

if success:
    print("Odometry successful!")
    print(f"Transformation:\n{trans}")

    target_pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
        target_rgbd, intrinsic)
    source_pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
        source_rgbd, intrinsic)
    source_pcd.transform(trans)
    o3d.visualization.draw_geometries([target_pcd, source_pcd])

print("\n" + "=" * 60)
print("STEP 4: Global Registration (FPFH + RANSAC)")
print("=" * 60)

voxel_size = 0.05


def preprocess_point_cloud(pcd, voxel_size):
    """Downsample, estimate normals, compute FPFH features."""
    print(f":: Downsample with voxel size {voxel_size:.3f}")
    pcd_down = pcd.voxel_down_sample(voxel_size)

    radius_normal = voxel_size * 2
    print(f":: Estimate normals with search radius {radius_normal:.3f}")
    pcd_down.estimate_normals(
        o3d.geometry.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30))

    radius_feature = voxel_size * 5
    print(f":: Compute FPFH features with search radius {radius_feature:.3f}")
    pcd_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
        pcd_down,
        o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100))
    return pcd_down, pcd_fpfh


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

trans_init = np.asarray([[0.0, 0.0, 1.0, 0.0],
                         [1.0, 0.0, 0.0, 0.0],
                         [0.0, 1.0, 0.0, 0.0],
                         [0.0, 0.0, 0.0, 1.0]])
source.transform(trans_init)

source_down, source_fpfh = preprocess_point_cloud(source, voxel_size)
target_down, target_fpfh = preprocess_point_cloud(target, voxel_size)

distance_threshold = voxel_size * 1.5
print(f":: RANSAC registration with distance threshold {distance_threshold:.3f}")

result_ransac = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
    source_down, target_down, source_fpfh, target_fpfh, True,
    distance_threshold,
    o3d.pipelines.registration.TransformationEstimationPointToPoint(False),
    3, [
        o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9),
        o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(
            distance_threshold)
    ], o3d.pipelines.registration.RANSACConvergenceCriteria(100000, 0.999))

print(f"RANSAC Result:")
print(f"  Fitness: {result_ransac.fitness:.6f}")
print(f"  Inlier RMSE: {result_ransac.inlier_rmse:.6f}")
print(f"  Correspondences: {len(result_ransac.correspondence_set)}")
draw_registration_result(source_down, target_down, result_ransac.transformation)

You now have a rough global alignment between two point clouds that started in completely different orientations. Next, you will tighten this alignment to sub-centimetre accuracy using point-to-plane ICP.

Refine Alignment with Point-to-Plane ICP

Your RANSAC registration produced a rough alignment by matching FPFH features on downsampled point clouds. The result is approximate because RANSAC operates on coarse 5cm voxels and relies on random sampling.

In this step, you will tighten that alignment using ICP (Iterative Closest Point) with point-to-plane constraints. ICP iteratively minimizes the distance between corresponding points on the full-resolution clouds, converging to sub-centimeter accuracy.

In this step, get ready to:
  • Apply point-to-plane ICP using the RANSAC result as initialization.
  • Compare registration metrics before and after refinement.
  • Understand why global and local registration complement each other.
Apply point-to-plane ICP refinement

Point-to-plane ICP constrains each point's motion along the surface normal of its nearest neighbor. This converges faster than point-to-point ICP because it allows sliding along flat surfaces while penalizing motion through them.

  • In reconstruction_pipeline.py, scroll to the bottom of the file after the RANSAC visualization call.
  • Add the ICP refinement code below the last line by copying the following:
print("\n" + "=" * 60)
print("STEP 5: ICP Refinement (Point-to-Plane)")
print("=" * 60)

# Use a strict distance threshold: voxel_size * 0.4 = 0.02m
distance_threshold_icp = voxel_size * 0.4
print(f":: Point-to-plane ICP with distance threshold {distance_threshold_icp:.3f}")

# Run ICP using RANSAC result as initialization
result_icp = o3d.pipelines.registration.registration_icp(
    source, target, distance_threshold_icp, result_ransac.transformation,
    o3d.pipelines.registration.TransformationEstimationPointToPlane())

print(f"ICP Result:")
print(f"  Fitness: {result_icp.fitness:.6f}")
print(f"  Inlier RMSE: {result_icp.inlier_rmse:.6f}")
print(f"  Correspondences: {len(result_icp.correspondence_set)}")
draw_registration_result(source, target, result_icp.transformation)

What does this code do?

distance_threshold_icp = voxel_size * 0.4 sets a strict 0.02m correspondence distance. Only point pairs within 2cm are considered matches. This is much tighter than the RANSAC threshold of 0.075m.

registration_icp() takes the full-resolution source and target clouds (not downsampled), the strict threshold, and the RANSAC transformation as its starting point.

TransformationEstimationPointToPlane() uses surface normals to minimize the distance from each source point to the tangent plane of its target correspondence. This is what gives point-to-plane ICP its speed advantage.

  • Save reconstruction_pipeline.py.
  • Run the script in your terminal. After the RANSAC visualization closes, you should see the ICP metrics printed and a new visualization window with a noticeably tighter alignment between the orange and blue point clouds.

Seeing a poor ICP result?

If your ICP fitness is near zero, check that you are passing result_ransac.transformation as the fourth argument. Without a good initialization, ICP gets stuck in a local minimum and reports almost no inlier correspondences.

Also confirm you are passing source and target (full resolution), not source_down and target_down. ICP needs the dense point clouds to achieve fine alignment.

Why is my ICP fitness near zero even though RANSAC worked?

Compare registration metrics before and after ICP

Your terminal output now shows metrics for both RANSAC and ICP. Two numbers tell the story of what ICP improved.

  • Look at the Fitness values. Fitness measures the fraction of inlier correspondences relative to the number of points in the target. A value near 0.62 means about 62% of target points have a close match in the source.
  • Look at the Inlier RMSE values. RANSAC typically reports an RMSE around 0.037m (3.7cm). ICP should drop this to around 0.007m (7mm). That is a 5x improvement in alignment accuracy.

Why does fitness drop while RMSE improves?

RANSAC uses a loose 0.075m threshold, so more point pairs qualify as inliers (higher fitness). ICP uses a strict 0.02m threshold, so fewer pairs qualify. The pairs that do qualify are much more precisely aligned.

Think of it this way: RANSAC casts a wide net and catches many approximate matches. ICP uses a fine sieve and keeps only the tightest correspondences.

Understand why both methods are needed together

The visualization window shows the refined alignment. Close it when you are done inspecting. Notice how the orange and blue clouds overlap with almost no visible gap between surfaces.

Why not use ICP alone?

ICP is a local optimization method. It iteratively moves each source point toward its closest target point. If the initial alignment is too far off, ICP converges to a wrong local minimum instead of the correct global alignment.

RANSAC solves this by finding an approximate alignment without any initialization. It matches geometric features (FPFH descriptors) and uses random sampling to find a consensus transformation. The result is coarse but globally correct.

The combination is the standard approach in production 3D reconstruction systems. RANSAC provides the "where are we roughly" answer. ICP provides the "exactly how do these surfaces align" answer.

Why point-to-plane instead of point-to-point?

Point-to-point ICP minimizes the distance between matched points directly. Point-to-plane ICP minimizes the distance from each source point to the tangent plane at its target correspondence. This means points can slide freely along flat surfaces while being pulled toward the correct depth.

The result is faster convergence (fewer iterations to reach the same accuracy) because the constraint better matches real-world surface geometry.

✔️ Awesome, I've got everything!

Great. Double check you have saved reconstruction_pipeline.py with the ICP refinement code at the bottom.

ⓧ I'd like to double check the full code

Here is the full reconstruction_pipeline.py up to this point:

import open3d as o3d
import numpy as np
import copy


class CameraPose:
    def __init__(self, meta, mat):
        self.metadata = meta
        self.pose = mat

    def __str__(self):
        return 'Metadata : ' + ' '.join(map(str, self.metadata)) + '\n' + \
            "Pose : " + "\n" + np.array_str(self.pose)


def read_trajectory(filename):
    traj = []
    with open(filename, 'r') as f:
        metastr = f.readline()
        while metastr:
            metadata = list(map(int, metastr.split()))
            mat = np.zeros(shape=(4, 4))
            for i in range(4):
                matstr = f.readline()
                mat[i, :] = np.fromstring(matstr, dtype=float, sep=' \t')
            traj.append(CameraPose(metadata, mat))
            metastr = f.readline()
    return traj


def draw_registration_result(source, target, transformation):
    source_temp = copy.deepcopy(source)
    target_temp = copy.deepcopy(target)
    source_temp.paint_uniform_color([1, 0.706, 0])
    target_temp.paint_uniform_color([0, 0.651, 0.929])
    source_temp.transform(transformation)
    o3d.visualization.draw_geometries([source_temp, target_temp])


print("=" * 60)
print("STEP 2: Loading RGBD Data and Creating Point Clouds")
print("=" * 60)

redwood_rgbd = o3d.data.SampleRedwoodRGBDImages()

color_raw = o3d.io.read_image(redwood_rgbd.color_paths[0])
depth_raw = o3d.io.read_image(redwood_rgbd.depth_paths[0])

rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
    color_raw, depth_raw)
print(f"RGBD Image: {rgbd_image}")

intrinsic = o3d.camera.PinholeCameraIntrinsic(
    o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault)
print(f"Camera intrinsic matrix:\n{intrinsic.intrinsic_matrix}")

pcd = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd_image, intrinsic)
pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])
print(f"Point cloud has {len(pcd.points)} points")
o3d.visualization.draw_geometries([pcd])

camera_poses = read_trajectory(redwood_rgbd.odometry_log_path)
print(f"Loaded {len(camera_poses)} camera poses")

print("\n" + "=" * 60)
print("STEP 3: Computing RGBD Odometry")
print("=" * 60)

source_color = o3d.io.read_image(redwood_rgbd.color_paths[0])
source_depth = o3d.io.read_image(redwood_rgbd.depth_paths[0])
target_color = o3d.io.read_image(redwood_rgbd.color_paths[1])
target_depth = o3d.io.read_image(redwood_rgbd.depth_paths[1])

source_rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(
    source_color, source_depth)
target_rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(
    target_color, target_depth)

option = o3d.pipelines.odometry.OdometryOption()
print(f"Odometry options: {option}")

odo_init = np.identity(4)
[success, trans, info] = o3d.pipelines.odometry.compute_rgbd_odometry(
    source_rgbd, target_rgbd, intrinsic, odo_init,
    o3d.pipelines.odometry.RGBDOdometryJacobianFromHybridTerm(), option)

if success:
    print("Odometry successful!")
    print(f"Transformation:\n{trans}")

    target_pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
        target_rgbd, intrinsic)
    source_pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
        source_rgbd, intrinsic)
    source_pcd.transform(trans)
    o3d.visualization.draw_geometries([target_pcd, source_pcd])

print("\n" + "=" * 60)
print("STEP 4: Global Registration (FPFH + RANSAC)")
print("=" * 60)

voxel_size = 0.05


def preprocess_point_cloud(pcd, voxel_size):
    """Downsample, estimate normals, compute FPFH features."""
    print(f":: Downsample with voxel size {voxel_size:.3f}")
    pcd_down = pcd.voxel_down_sample(voxel_size)

    radius_normal = voxel_size * 2
    print(f":: Estimate normals with search radius {radius_normal:.3f}")
    pcd_down.estimate_normals(
        o3d.geometry.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30))

    radius_feature = voxel_size * 5
    print(f":: Compute FPFH features with search radius {radius_feature:.3f}")
    pcd_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
        pcd_down,
        o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100))
    return pcd_down, pcd_fpfh


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

trans_init = np.asarray([[0.0, 0.0, 1.0, 0.0],
                         [1.0, 0.0, 0.0, 0.0],
                         [0.0, 1.0, 0.0, 0.0],
                         [0.0, 0.0, 0.0, 1.0]])
source.transform(trans_init)

source_down, source_fpfh = preprocess_point_cloud(source, voxel_size)
target_down, target_fpfh = preprocess_point_cloud(target, voxel_size)

distance_threshold = voxel_size * 1.5
print(f":: RANSAC registration with distance threshold {distance_threshold:.3f}")

result_ransac = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
    source_down, target_down, source_fpfh, target_fpfh, True,
    distance_threshold,
    o3d.pipelines.registration.TransformationEstimationPointToPoint(False),
    3, [
        o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9),
        o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(
            distance_threshold)
    ], o3d.pipelines.registration.RANSACConvergenceCriteria(100000, 0.999))

print(f"RANSAC Result:")
print(f"  Fitness: {result_ransac.fitness:.6f}")
print(f"  Inlier RMSE: {result_ransac.inlier_rmse:.6f}")
print(f"  Correspondences: {len(result_ransac.correspondence_set)}")
draw_registration_result(source_down, target_down, result_ransac.transformation)

print("\n" + "=" * 60)
print("STEP 5: ICP Refinement (Point-to-Plane)")
print("=" * 60)

distance_threshold_icp = voxel_size * 0.4
print(f":: Point-to-plane ICP with distance threshold {distance_threshold_icp:.3f}")

result_icp = o3d.pipelines.registration.registration_icp(
    source, target, distance_threshold_icp, result_ransac.transformation,
    o3d.pipelines.registration.TransformationEstimationPointToPlane())

print(f"ICP Result:")
print(f"  Fitness: {result_icp.fitness:.6f}")
print(f"  Inlier RMSE: {result_icp.inlier_rmse:.6f}")
print(f"  Correspondences: {len(result_icp.correspondence_set)}")
draw_registration_result(source, target, result_icp.transformation)

Your point clouds are now aligned with millimeter-level precision. Next up, you will fuse all five RGB-D frames into a single volumetric representation and extract a smooth 3D triangle mesh.

Integrate TSDF Volume and Extract Mesh

You have aligned point clouds using both global registration and local ICP refinement. The missing piece is fusing all of these aligned depth observations into a single, clean 3D surface.

In this step, you will integrate all 5 RGB-D frames into a TSDF (Truncated Signed Distance Function) volume. The TSDF stores a weighted distance-to-surface value in every voxel, which naturally averages out sensor noise across multiple observations. You will then extract a smooth triangle mesh from this volume using marching cubes.

In this step, get ready to:
  • Create a ScalableTSDFVolume with millimeter-scale resolution.
  • Integrate all RGB-D frames into the volume using known camera poses.
  • Extract and visualize the reconstructed 3D triangle mesh.
Create a ScalableTSDFVolume

A TSDF volume divides 3D space into a grid of tiny cubes (voxels). Each voxel stores the signed distance to the nearest surface, truncated beyond a threshold. When multiple depth frames observe the same region, their distance values are averaged together, which smooths out sensor noise.

  • In reconstruction_pipeline.py, add the following code at the bottom of the file to create the TSDF volume:
print("\n" + "=" * 60)
print("STEP 6: TSDF Integration and Mesh Reconstruction")
print("=" * 60)

# Create ScalableTSDFVolume
# voxel_length = 4.0/512.0 ~ 7.8mm resolution
# sdf_trunc = 0.04m truncation distance
volume = o3d.pipelines.integration.ScalableTSDFVolume(
    voxel_length=4.0 / 512.0,
    sdf_trunc=0.04,
    color_type=o3d.pipelines.integration.TSDFVolumeColorType.RGB8)

What does this code do?

ScalableTSDFVolume uses hierarchical hashing to allocate voxels only where surfaces exist, making it memory-efficient for large scenes.

  • voxel_length=4.0 / 512.0 sets each voxel to approximately 7.8mm. Smaller voxels mean finer detail but more memory.
  • sdf_trunc=0.04 truncates the distance field at 4cm from the surface. Observations beyond this range are ignored for that voxel.
  • color_type=TSDFVolumeColorType.RGB8 stores 8-bit RGB color alongside the distance values, so the final mesh will be textured.
  • Save reconstruction_pipeline.py.

You won't see a change yet. The volume is an empty container waiting for depth data in the next chunk.

Integrate all RGB-D frames into the volume

Each frame contributes depth observations from a different camera position. The integrate() call projects each pixel into the 3D volume and updates the signed distance at every voxel near the observed surface. The camera extrinsic matrix tells the volume where in world-space each pixel lands.

  • Add the integration loop below the volume creation code:
# Integrate all frames using known camera poses
for i in range(len(camera_poses)):
    print(f"Integrating frame {i}...")
    color = o3d.io.read_image(redwood_rgbd.color_paths[i])
    depth = o3d.io.read_image(redwood_rgbd.depth_paths[i])
    rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(
        color, depth, depth_trunc=4.0, convert_rgb_to_intensity=False)
    volume.integrate(
        rgbd, intrinsic, np.linalg.inv(camera_poses[i].pose))

What does this code do?

  • The loop iterates through all 5 frames in the Redwood dataset.
  • depth_trunc=4.0 discards depth readings beyond 4 meters. Far-away surfaces are too noisy to be useful.
  • convert_rgb_to_intensity=False keeps the full RGB color channels instead of converting to grayscale.
  • np.linalg.inv(camera_poses[i].pose) converts the camera-to-world pose into a world-to-camera extrinsic. Open3D's integrate() expects the extrinsic (inverse of the camera pose).
  • Save reconstruction_pipeline.py.

You won't see visual output yet. The integration loop populates the volume internally. The next chunk extracts the mesh and opens a visualizer.

Seeing a key error or index out of range?

Make sure you did not remove or overwrite the camera_poses variable from Step 2. The loop relies on the trajectory loaded earlier via read_trajectory().

help me debug my TSDF integration loop

Extract the triangle mesh and visualize

The TSDF volume now contains a weighted 3D average of all depth observations. The marching cubes algorithm finds the zero-crossing of the distance field (where distance = 0 is the surface) and generates triangles that form a smooth mesh.

  • Add the mesh extraction and visualization code below the integration loop:
# Extract triangle mesh via marching cubes
print("Extracting triangle mesh...")
mesh = volume.extract_triangle_mesh()
mesh.compute_vertex_normals()
print(f"Reconstructed mesh: {len(mesh.vertices)} vertices, "
      f"{len(mesh.triangles)} triangles")

o3d.visualization.draw_geometries([mesh],
                                  front=[0.5297, -0.1873, -0.8272],
                                  lookat=[2.0712, 2.0312, 1.7251],
                                  up=[-0.0558, -0.9809, 0.1864],
                                  zoom=0.47)

print("\n" + "=" * 60)
print("PIPELINE COMPLETE!")
print("=" * 60)

What does this code do?

  • extract_triangle_mesh() runs the marching cubes algorithm internally. It walks through every voxel and generates triangles wherever the signed distance crosses zero.
  • compute_vertex_normals() calculates smooth surface normals at each vertex so the mesh renders with proper lighting and shading.
  • The front, lookat, up, and zoom parameters set a preset camera angle so the reconstructed scene is immediately visible without manual rotation.
  • Save reconstruction_pipeline.py.
  • Run the full pipeline by executing:
python reconstruction_pipeline.py

After closing each intermediate visualization window (from Steps 2-5), you should see a colored 3D mesh of an indoor scene appear in the final viewer. The terminal prints the vertex and triangle counts, followed by PIPELINE COMPLETE!.

Why does TSDF fusion produce a smoother mesh than raw point clouds?

Each individual depth frame has sensor noise. When multiple frames observe the same surface from slightly different angles, the TSDF averaging cancels out random noise. The result is a surface that is smoother and more accurate than any single observation.

This is the same principle used in production SLAM systems, robotic mapping, and AR/VR scene reconstruction. The difference between a research prototype and a production system is scale (millions of frames) and real-time constraints.

Visualization window not appearing?

Open3D visualization requires a display. If you are running on a headless server or remote SSH session, the GLFW window cannot open. Run the script on your local machine instead.

If the window appears but looks blank, try clicking and dragging to rotate the view. The preset camera angle works for most setups but your window size may differ.

help me troubleshoot Open3D visualization

✔️ Awesome, I've got everything!

Great. Double check you have saved reconstruction_pipeline.py and confirmed the 3D mesh visualizes correctly.

ⓧ I'd like to double check the full code

Here is the complete reconstruction_pipeline.py file for comparison:

import open3d as o3d
import numpy as np
import copy


class CameraPose:
    def __init__(self, meta, mat):
        self.metadata = meta
        self.pose = mat

    def __str__(self):
        return 'Metadata : ' + ' '.join(map(str, self.metadata)) + '\n' + \
            "Pose : " + "\n" + np.array_str(self.pose)


def read_trajectory(filename):
    traj = []
    with open(filename, 'r') as f:
        metastr = f.readline()
        while metastr:
            metadata = list(map(int, metastr.split()))
            mat = np.zeros(shape=(4, 4))
            for i in range(4):
                matstr = f.readline()
                mat[i, :] = np.fromstring(matstr, dtype=float, sep=' \t')
            traj.append(CameraPose(metadata, mat))
            metastr = f.readline()
    return traj


def draw_registration_result(source, target, transformation):
    source_temp = copy.deepcopy(source)
    target_temp = copy.deepcopy(target)
    source_temp.paint_uniform_color([1, 0.706, 0])
    target_temp.paint_uniform_color([0, 0.651, 0.929])
    source_temp.transform(transformation)
    o3d.visualization.draw_geometries([source_temp, target_temp])


print("=" * 60)
print("STEP 2: Loading RGBD Data and Creating Point Clouds")
print("=" * 60)

redwood_rgbd = o3d.data.SampleRedwoodRGBDImages()

color_raw = o3d.io.read_image(redwood_rgbd.color_paths[0])
depth_raw = o3d.io.read_image(redwood_rgbd.depth_paths[0])

rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
    color_raw, depth_raw)
print(f"RGBD Image: {rgbd_image}")

intrinsic = o3d.camera.PinholeCameraIntrinsic(
    o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault)
print(f"Camera intrinsic matrix:\n{intrinsic.intrinsic_matrix}")

pcd = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd_image, intrinsic)
pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])
print(f"Point cloud has {len(pcd.points)} points")
o3d.visualization.draw_geometries([pcd])

camera_poses = read_trajectory(redwood_rgbd.odometry_log_path)
print(f"Loaded {len(camera_poses)} camera poses")

print("\n" + "=" * 60)
print("STEP 3: Computing RGBD Odometry")
print("=" * 60)

source_color = o3d.io.read_image(redwood_rgbd.color_paths[0])
source_depth = o3d.io.read_image(redwood_rgbd.depth_paths[0])
target_color = o3d.io.read_image(redwood_rgbd.color_paths[1])
target_depth = o3d.io.read_image(redwood_rgbd.depth_paths[1])

source_rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(
    source_color, source_depth)
target_rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(
    target_color, target_depth)

option = o3d.pipelines.odometry.OdometryOption()
print(f"Odometry options: {option}")

odo_init = np.identity(4)
[success, trans, info] = o3d.pipelines.odometry.compute_rgbd_odometry(
    source_rgbd, target_rgbd, intrinsic, odo_init,
    o3d.pipelines.odometry.RGBDOdometryJacobianFromHybridTerm(), option)

if success:
    print("Odometry successful!")
    print(f"Transformation:\n{trans}")

    target_pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
        target_rgbd, intrinsic)
    source_pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
        source_rgbd, intrinsic)
    source_pcd.transform(trans)
    o3d.visualization.draw_geometries([target_pcd, source_pcd])

print("\n" + "=" * 60)
print("STEP 4: Global Registration (FPFH + RANSAC)")
print("=" * 60)

voxel_size = 0.05


def preprocess_point_cloud(pcd, voxel_size):
    """Downsample, estimate normals, compute FPFH features."""
    print(f":: Downsample with voxel size {voxel_size:.3f}")
    pcd_down = pcd.voxel_down_sample(voxel_size)

    radius_normal = voxel_size * 2
    print(f":: Estimate normals with search radius {radius_normal:.3f}")
    pcd_down.estimate_normals(
        o3d.geometry.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30))

    radius_feature = voxel_size * 5
    print(f":: Compute FPFH features with search radius {radius_feature:.3f}")
    pcd_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
        pcd_down,
        o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100))
    return pcd_down, pcd_fpfh


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

trans_init = np.asarray([[0.0, 0.0, 1.0, 0.0],
                         [1.0, 0.0, 0.0, 0.0],
                         [0.0, 1.0, 0.0, 0.0],
                         [0.0, 0.0, 0.0, 1.0]])
source.transform(trans_init)

source_down, source_fpfh = preprocess_point_cloud(source, voxel_size)
target_down, target_fpfh = preprocess_point_cloud(target, voxel_size)

distance_threshold = voxel_size * 1.5
print(f":: RANSAC registration with distance threshold {distance_threshold:.3f}")

result_ransac = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
    source_down, target_down, source_fpfh, target_fpfh, True,
    distance_threshold,
    o3d.pipelines.registration.TransformationEstimationPointToPoint(False),
    3, [
        o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9),
        o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(
            distance_threshold)
    ], o3d.pipelines.registration.RANSACConvergenceCriteria(100000, 0.999))

print(f"RANSAC Result:")
print(f"  Fitness: {result_ransac.fitness:.6f}")
print(f"  Inlier RMSE: {result_ransac.inlier_rmse:.6f}")
print(f"  Correspondences: {len(result_ransac.correspondence_set)}")
draw_registration_result(source_down, target_down, result_ransac.transformation)

print("\n" + "=" * 60)
print("STEP 5: ICP Refinement (Point-to-Plane)")
print("=" * 60)

distance_threshold_icp = voxel_size * 0.4
print(f":: Point-to-plane ICP with distance threshold {distance_threshold_icp:.3f}")

result_icp = o3d.pipelines.registration.registration_icp(
    source, target, distance_threshold_icp, result_ransac.transformation,
    o3d.pipelines.registration.TransformationEstimationPointToPlane())

print(f"ICP Result:")
print(f"  Fitness: {result_icp.fitness:.6f}")
print(f"  Inlier RMSE: {result_icp.inlier_rmse:.6f}")
print(f"  Correspondences: {len(result_icp.correspondence_set)}")
draw_registration_result(source, target, result_icp.transformation)

print("\n" + "=" * 60)
print("STEP 6: TSDF Integration and Mesh Reconstruction")
print("=" * 60)

volume = o3d.pipelines.integration.ScalableTSDFVolume(
    voxel_length=4.0 / 512.0,
    sdf_trunc=0.04,
    color_type=o3d.pipelines.integration.TSDFVolumeColorType.RGB8)

for i in range(len(camera_poses)):
    print(f"Integrating frame {i}...")
    color = o3d.io.read_image(redwood_rgbd.color_paths[i])
    depth = o3d.io.read_image(redwood_rgbd.depth_paths[i])
    rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(
        color, depth, depth_trunc=4.0, convert_rgb_to_intensity=False)
    volume.integrate(
        rgbd, intrinsic, np.linalg.inv(camera_poses[i].pose))

print("Extracting triangle mesh...")
mesh = volume.extract_triangle_mesh()
mesh.compute_vertex_normals()
print(f"Reconstructed mesh: {len(mesh.vertices)} vertices, "
      f"{len(mesh.triangles)} triangles")

o3d.visualization.draw_geometries([mesh],
                                  front=[0.5297, -0.1873, -0.8272],
                                  lookat=[2.0712, 2.0312, 1.7251],
                                  up=[-0.0558, -0.9809, 0.1864],
                                  zoom=0.47)

print("\n" + "=" * 60)
print("PIPELINE COMPLETE!")
print("=" * 60)

Secret mission

Multi-Scale Colored ICP Registration

Standard point-to-plane ICP has a weakness: on flat, textureless surfaces it can "slip" because geometry alone cannot constrain tangent-plane motion. Colored ICP (Park et al., ICCV 2017) fixes this by jointly optimizing geometric AND photometric alignment. Implement a 3-scale coarse-to-fine colored ICP pipeline and prove it outperforms plain ICP on textured surfaces.

Clean Up Your Resources

Clean Up Your Resources

This project runs entirely locally with no cloud resources or paid APIs. There are no ongoing costs. Decide whether to keep your files and packages, or remove them entirely.

Resources you used:

  • reconstruction_pipeline.py (main pipeline script on your Desktop).
  • colored_icp_registration.py (Secret Mission script on your Desktop).
  • Python packages: open3d, numpy, matplotlib (installed via pip).
  • Open3D dataset cache stored in ~/open3d_data (~25MB of downloaded RGBD images and point clouds).

Keep everything running

No action needed. Choose this if you plan to extend the pipeline further or experiment with different datasets.

  • Your Python scripts remain on your Desktop, ready to run again.
  • The open3d package stays installed and available for future projects.
  • The cached datasets in ~/open3d_data save you re-downloading next time you use Open3D's built-in data.

Pause - I'll come back to this later

No running processes to stop. This project uses scripts you run manually, so there is nothing consuming memory or CPU in the background.

  • Your files and packages remain in place for when you return.
  • The dataset cache in ~/open3d_data takes up ~25MB. You can leave it or remove it now and let Open3D re-download it later.

Delete - I don't want to use this again

Remove all project files, packages, and cached data to start fresh.

  • Delete your project scripts by running:
cd ~/Desktop
rm reconstruction_pipeline.py colored_icp_registration.py
  • Remove the Open3D dataset cache by running:
rm -rf ~/open3d_data
  • Uninstall the Python packages by running:
pip uninstall open3d numpy matplotlib -y

Keep NumPy and Matplotlib?

NumPy and Matplotlib are common Python packages used across many projects. If you use them elsewhere, skip removing them and only uninstall open3d.

Nice Work!

Nice Work!

You have built a complete multi-view 3D scene reconstruction pipeline from scratch using Open3D. Starting from raw RGB-D image pairs, you computed camera motion, aligned point clouds, and fused everything into a final 3D triangle mesh.

You've learned how to:

  • Build an end-to-end pipeline that converts RGB-D frames into 3D point clouds and computes frame-to-frame camera motion using hybrid RGBD odometry.
  • Align point clouds using FPFH feature descriptors with RANSAC global registration, then refine with point-to-plane ICP for sub-centimeter accuracy.
  • Fuse multiple depth frames into a TSDF volume and extract a smooth 3D triangle mesh via marching cubes.
  • Implement multi-scale Colored ICP registration that exploits texture information for tighter alignment on planar surfaces.

Ready to quiz yourself?