Colored point cloud registration
This tutorial demonstrates an ICP variant that uses both geometry and color for registration. It implements the algorithm of [Park2017]. The color information locks the alignment along the tangent plane. Thus this algorithm is more accurate and more robust than prior point cloud registration algorithms, while the running speed is comparable to that of ICP registration. This tutorial uses notations from ICP registration.
5# examples/Python/Advanced/colored_pointcloud_registration.py
6
7import numpy as np
8import copy
9import open3d as o3d
10
11
12def draw_registration_result_original_color(source, target, transformation):
13 source_temp = copy.deepcopy(source)
14 source_temp.transform(transformation)
15 o3d.visualization.draw_geometries([source_temp, target])
16
17
18if __name__ == "__main__":
19
20 print("1. Load two point clouds and show initial pose")
21 source = o3d.io.read_point_cloud("../../TestData/ColoredICP/frag_115.ply")
22 target = o3d.io.read_point_cloud("../../TestData/ColoredICP/frag_116.ply")
23
24 # draw initial alignment
25 current_transformation = np.identity(4)
26 draw_registration_result_original_color(source, target,
27 current_transformation)
28
29 # point to plane ICP
30 current_transformation = np.identity(4)
31 print("2. Point-to-plane ICP registration is applied on original point")
32 print(" clouds to refine the alignment. Distance threshold 0.02.")
33 result_icp = o3d.registration.registration_icp(
34 source, target, 0.02, current_transformation,
35 o3d.registration.TransformationEstimationPointToPlane())
36 print(result_icp)
37 draw_registration_result_original_color(source, target,
38 result_icp.transformation)
39
40 # colored pointcloud registration
41 # This is implementation of following paper
42 # J. Park, Q.-Y. Zhou, V. Koltun,
43 # Colored Point Cloud Registration Revisited, ICCV 2017
44 voxel_radius = [0.04, 0.02, 0.01]
45 max_iter = [50, 30, 14]
46 current_transformation = np.identity(4)
47 print("3. Colored point cloud registration")
48 for scale in range(3):
49 iter = max_iter[scale]
50 radius = voxel_radius[scale]
51 print([iter, radius, scale])
52
53 print("3-1. Downsample with a voxel size %.2f" % radius)
54 source_down = source.voxel_down_sample(radius)
55 target_down = target.voxel_down_sample(radius)
56
57 print("3-2. Estimate normal.")
58 source_down.estimate_normals(
59 o3d.geometry.KDTreeSearchParamHybrid(radius=radius * 2, max_nn=30))
60 target_down.estimate_normals(
61 o3d.geometry.KDTreeSearchParamHybrid(radius=radius * 2, max_nn=30))
62
63 print("3-3. Applying colored point cloud registration")
64 result_icp = o3d.registration.registration_colored_icp(
65 source_down, target_down, radius, current_transformation,
66 o3d.registration.ICPConvergenceCriteria(relative_fitness=1e-6,
67 relative_rmse=1e-6,
68 max_iteration=iter))
69 current_transformation = result_icp.transformation
70 print(result_icp)
71 draw_registration_result_original_color(source, target,
72 result_icp.transformation)
Helper visualization function
12def draw_registration_result_original_color(source, target, transformation):
13 source_temp = copy.deepcopy(source)
14 source_temp.transform(transformation)
15 o3d.visualization.draw_geometries([source_temp, target])
In order to demonstrate the alignment between colored point clouds, draw_registration_result_original_color
renders point clouds with their original color.
Input
20 print("1. Load two point clouds and show initial pose")
21 source = o3d.io.read_point_cloud("../../TestData/ColoredICP/frag_115.ply")
22 target = o3d.io.read_point_cloud("../../TestData/ColoredICP/frag_116.ply")
23
24 # draw initial alignment
25 current_transformation = np.identity(4)
26 draw_registration_result_original_color(source, target,
27 current_transformation)
This script reads a source point cloud and a target point cloud from two files. An identity matrix is used as initialization.


Point-to-plane ICP
29 # point to plane ICP
30 current_transformation = np.identity(4)
31 print("2. Point-to-plane ICP registration is applied on original point")
32 print(" clouds to refine the alignment. Distance threshold 0.02.")
33 result_icp = o3d.registration.registration_icp(
34 source, target, 0.02, current_transformation,
35 o3d.registration.TransformationEstimationPointToPlane())
36 print(result_icp)
37 draw_registration_result_original_color(source, target,
38 result_icp.transformation)
We first run Point-to-plane ICP as a baseline approach. The visualization below shows misaligned green triangle textures. This is because geometric constraint does not prevent two planar surfaces from slipping.


Colored point cloud registration
The core function for colored point cloud registration is registration_colored_icp
. Following [Park2017], it runs ICP iterations (see Point-to-point ICP for details) with a joint optimization objective
where \(\mathbf{T}\) is the transformation matrix to be estimated. \(E_{C}\) and \(E_{G}\) are the photometric and geometric terms, respectively. \(\delta\in[0,1]\) is a weight parameter that has been determined empirically.
The geometric term \(E_{G}\) is the same as the Point-to-plane ICP objective
where \(\mathcal{K}\) is the correspondence set in the current iteration. \(\mathbf{n}_{\mathbf{p}}\) is the normal of point \(\mathbf{p}\).
The color term \(E_{C}\) measures the difference between the color of point \(\mathbf{q}\) (denoted as \(C(\mathbf{q})\)) and the color of its projection on the tangent plane of \(\mathbf{p}\).
where \(C_{\mathbf{p}}(\cdot)\) is a precomputed function continuously defined on the tangent plane of \(\mathbf{p}\). Function \(\mathbf{f}(\cdot)\) projects a 3D point to the tangent plane. More details refer to [Park2017].
To further improve efficiency, [Park2017] proposes a multi-scale registration scheme. This has been implemented in the following script.
40 # colored pointcloud registration
41 # This is implementation of following paper
42 # J. Park, Q.-Y. Zhou, V. Koltun,
43 # Colored Point Cloud Registration Revisited, ICCV 2017
44 voxel_radius = [0.04, 0.02, 0.01]
45 max_iter = [50, 30, 14]
46 current_transformation = np.identity(4)
47 print("3. Colored point cloud registration")
48 for scale in range(3):
49 iter = max_iter[scale]
50 radius = voxel_radius[scale]
51 print([iter, radius, scale])
52
53 print("3-1. Downsample with a voxel size %.2f" % radius)
54 source_down = source.voxel_down_sample(radius)
55 target_down = target.voxel_down_sample(radius)
56
57 print("3-2. Estimate normal.")
58 source_down.estimate_normals(
59 o3d.geometry.KDTreeSearchParamHybrid(radius=radius * 2, max_nn=30))
60 target_down.estimate_normals(
61 o3d.geometry.KDTreeSearchParamHybrid(radius=radius * 2, max_nn=30))
62
63 print("3-3. Applying colored point cloud registration")
64 result_icp = o3d.registration.registration_colored_icp(
65 source_down, target_down, radius, current_transformation,
66 o3d.registration.ICPConvergenceCriteria(relative_fitness=1e-6,
67 relative_rmse=1e-6,
68 max_iteration=iter))
69 current_transformation = result_icp.transformation
70 print(result_icp)
71 draw_registration_result_original_color(source, target,
72 result_icp.transformation)
In total, 3 layers of multi-resolution point clouds are created with Voxel downsampling. Normals are computed with Vertex normal estimation. The core registration function registration_colored_icp
is called for each layer, from coarse to fine. lambda_geometric
is an optional argument for registration_colored_icp
that determines \(\lambda \in [0,1]\) in the overall energy \(\lambda E_{G} + (1-\lambda) E_{C}\).
The output is a tight alignment of the two point clouds. Notice the green triangles on the wall.

