Pipelines#
colored_icp_registration.py#
1# ----------------------------------------------------------------------------
2# - CloudViewer: www.cloudViewer.org -
3# ----------------------------------------------------------------------------
4# Copyright (c) 2018-2024 www.cloudViewer.org
5# SPDX-License-Identifier: MIT
6# ----------------------------------------------------------------------------
7"""ICP variant that uses both geometry and color for registration"""
8
9import cloudViewer as cv3d
10import numpy as np
11import copy
12
13
14def draw_registration_result(source, target, transformation):
15 source_temp = copy.deepcopy(source)
16 source_temp.transform(transformation)
17 cv3d.visualization.draw([source_temp, target])
18
19
20print("Load two point clouds and show initial pose ...")
21ply_data = cv3d.data.DemoColoredICPPointClouds()
22source = cv3d.io.read_point_cloud(ply_data.paths[0])
23target = cv3d.io.read_point_cloud(ply_data.paths[1])
24
25if __name__ == "__main__":
26 # Draw initial alignment.
27 current_transformation = np.identity(4)
28 # draw_registration_result(source, target, current_transformation)
29 print(current_transformation)
30
31 # Colored pointcloud registration.
32 # This is implementation of following paper:
33 # J. Park, Q.-Y. Zhou, V. Koltun,
34 # Colored Point Cloud Registration Revisited, ICCV 2017.
35 voxel_radius = [0.04, 0.02, 0.01]
36 max_iter = [50, 30, 14]
37 current_transformation = np.identity(4)
38 print("Colored point cloud registration ...\n")
39 for scale in range(3):
40 iter = max_iter[scale]
41 radius = voxel_radius[scale]
42 print([iter, radius, scale])
43
44 print("1. Downsample with a voxel size %.2f" % radius)
45 source_down = source.voxel_down_sample(radius)
46 target_down = target.voxel_down_sample(radius)
47
48 print("2. Estimate normal")
49 source_down.estimate_normals(
50 cv3d.geometry.KDTreeSearchParamHybrid(radius=radius * 2, max_nn=30))
51 target_down.estimate_normals(
52 cv3d.geometry.KDTreeSearchParamHybrid(radius=radius * 2, max_nn=30))
53
54 print("3. Applying colored point cloud registration")
55 result_icp = cv3d.pipelines.registration.registration_colored_icp(
56 source_down, target_down, radius, current_transformation,
57 cv3d.pipelines.registration.TransformationEstimationForColoredICP(),
58 cv3d.pipelines.registration.ICPConvergenceCriteria(
59 relative_fitness=1e-6, relative_rmse=1e-6, max_iteration=iter))
60 current_transformation = result_icp.transformation
61 print(result_icp, "\n")
62 # draw_registration_result(source, target, result_icp.transformation)
63 print(current_transformation)
doppler_icp_registration.py#
1# ----------------------------------------------------------------------------
2# - CloudViewer: www.cloudViewer.org -
3# ----------------------------------------------------------------------------
4# Copyright (c) 2018-2024 www.cloudViewer.org
5# SPDX-License-Identifier: MIT
6# ----------------------------------------------------------------------------
7"""Example script to run Doppler ICP point cloud registration.
8
9This script runs Doppler ICP and point-to-plane ICP on DemoDopplerICPSequence.
10
11This is the implementation of the following paper:
12B. Hexsel, H. Vhavle, Y. Chen,
13DICP: Doppler Iterative Closest Point Algorithm, RSS 2022.
14
15Usage:
16python doppler_icp_registration.py [-h] \
17 --source SOURCE --target TARGET [--device {cpu,cuda}]
18"""
19
20import argparse
21import json
22import os
23
24import numpy as np
25import cloudViewer as cv3d
26import cloudViewer.t.pipelines.registration as o3d_reg
27from pyquaternion import Quaternion
28
29
30def translation_quaternion_to_transform(translation,
31 quaternion,
32 inverse=False,
33 quat_xyzw=False):
34 """Converts translation and WXYZ quaternion to a transformation matrix.
35
36 Args:
37 translation: (3,) ndarray representing the translation vector.
38 quaternion: (4,) ndarray representing the quaternion.
39 inverse: If True, returns the inverse transformation.
40 quat_xyzw: If True, this indicates that quaternion is in XYZW format.
41
42 Returns:
43 (4, 4) ndarray representing the transformation matrix.
44 """
45 if quat_xyzw:
46 quaternion = np.roll(quaternion, 1)
47 transform = Quaternion(quaternion).transformation_matrix # [w, x, y, z]
48 transform[:3, -1] = translation # [x, y, z]
49 return np.linalg.inv(transform) if inverse else transform
50
51
52def load_tum_file(filename):
53 """Loads poses in TUM RGBD format: [timestamp, x, y, z, qx, qy, qz, qw].
54
55 Args:
56 filename (string): Path to the TUM poses file.
57
58 Returns:
59 A tuple containing an array of 4x4 poses and timestamps.
60 """
61 # Load the TUM text file.
62 data = np.loadtxt(filename, delimiter=' ')
63 print('Loaded %d poses from %s (%.2f secs)' %
64 (len(data), os.path.basename(filename), data[-1][0] - data[0][0]))
65
66 # Parse timestamps and poses.
67 timestamps = data[:, 0]
68 poses = np.array([
69 translation_quaternion_to_transform(tq[:3], tq[3:], quat_xyzw=True)
70 for tq in data[:, 1:]
71 ])
72 return poses, timestamps
73
74
75def get_calibration(demo_sequence):
76 """Returns the vehicle to sensor calibration transformation and the time
77 period (in secs) between sequential point cloud scans.
78
79 Args:
80 demo_sequence (DemoDopplerICPSequence): Doppler ICP dataset.
81
82 Returns:
83 A tuple of 4x4 array representing the transform, and the period.
84 """
85 with open(demo_sequence.calibration_path) as f:
86 data = json.load(f)
87
88 transform_vehicle_to_sensor = np.array(
89 data['transform_vehicle_to_sensor']).reshape(4, 4)
90 period = data['period']
91
92 return transform_vehicle_to_sensor, period
93
94
95def get_trajectory(demo_sequence):
96 """Returns the ground truth trajectory of the dataset.
97
98 Args:
99 demo_sequence (DemoDopplerICPSequence): Doppler ICP dataset.
100
101 Returns:
102 An array of 4x4 poses for this sequence.
103 """
104 return load_tum_file(demo_sequence.trajectory_path)[0]
105
106
107def get_ground_truth_pose(demo_sequence, source_idx, target_idx):
108 """Returns the ground truth poses from the dataset.
109
110 Args:
111 demo_sequence (DemoDopplerICPSequence): Doppler ICP dataset.
112 source_idx (int): Index of the source point cloud pose.
113 target_idx (int): Index of the target point cloud pose.
114
115 Returns:
116 4x4 array representing the transformation between target and source.
117 """
118 poses = get_trajectory(demo_sequence)
119 return np.linalg.inv(poses[target_idx]) @ poses[source_idx]
120
121
122def run_doppler_icp(args):
123 """Runs Doppler ICP on a given pair of point clouds.
124
125 Args:
126 args: Command line arguments.
127 """
128 # Setup data type and device.
129 dtype = cv3d.core.float32
130 device = cv3d.core.Device('CUDA:0' if args.device == 'cuda' else 'CPU:0')
131
132 # Load the point clouds.
133 demo_sequence = cv3d.data.DemoDopplerICPSequence()
134 source = cv3d.t.io.read_point_cloud(demo_sequence.paths[args.source])
135 target = cv3d.t.io.read_point_cloud(demo_sequence.paths[args.target])
136
137 # Load the calibration parameters.
138 transform_vehicle_to_sensor, period = get_calibration(demo_sequence)
139
140 # Downsample the pointcloud.
141 source_in_S = source.uniform_down_sample(5)
142 target_in_S = target.uniform_down_sample(5)
143
144 # Transform the Open3D point cloud from sensor to vehicle frame.
145 source_in_V = source_in_S.to(device).transform(transform_vehicle_to_sensor)
146 target_in_V = target_in_S.to(device).transform(transform_vehicle_to_sensor)
147
148 # Move tensor to device.
149 init_transform = cv3d.core.Tensor(np.eye(4), device=device)
150 transform_vehicle_to_sensor = cv3d.core.Tensor(transform_vehicle_to_sensor,
151 device=device)
152
153 # Compute normals for target.
154 target_in_V.estimate_normals(radius=10.0, max_nn=30)
155
156 # Compute direction vectors on source point cloud frame in sensor frame.
157 directions = source_in_S.point.positions.numpy()
158 norms = np.tile(np.linalg.norm(directions, axis=1), (3, 1)).T
159 directions = directions / norms
160 source_in_V.point['directions'] = cv3d.core.Tensor(directions, dtype,
161 device)
162
163 # Setup robust kernels.
164 kernel = o3d_reg.robust_kernel.RobustKernel(o3d_reg.robust_kernel.TukeyLoss,
165 scaling_parameter=0.5)
166
167 # Setup convergence criteria.
168 criteria = o3d_reg.ICPConvergenceCriteria(relative_fitness=1e-6,
169 relative_rmse=1e-6,
170 max_iteration=200)
171
172 # Setup transformation estimator.
173 estimator_p2l = o3d_reg.TransformationEstimationPointToPlane(kernel)
174 estimator_dicp = o3d_reg.TransformationEstimationForDopplerICP(
175 period=period * (args.target - args.source),
176 lambda_doppler=0.01,
177 reject_dynamic_outliers=False,
178 doppler_outlier_threshold=2.0,
179 outlier_rejection_min_iteration=2,
180 geometric_robust_loss_min_iteration=0,
181 doppler_robust_loss_min_iteration=2,
182 goemetric_kernel=kernel,
183 doppler_kernel=kernel,
184 transform_vehicle_to_sensor=transform_vehicle_to_sensor)
185
186 # Run Doppler ICP and point-to-plane ICP registration for comparison.
187 max_neighbor_distance = 0.3
188 results = [
189 o3d_reg.icp(source_in_V, target_in_V, max_neighbor_distance,
190 init_transform, estimator, criteria)
191 for estimator in [estimator_p2l, estimator_dicp]
192 ]
193
194 # Display the poses.
195 np.set_printoptions(suppress=True, precision=4)
196 print('Estimated pose from Point-to-Plane ICP [%s iterations]:' %
197 results[0].num_iterations)
198 print(results[0].transformation.numpy())
199
200 print('\nEstimated pose from Doppler ICP [%s iterations]:' %
201 results[1].num_iterations)
202 print(results[1].transformation.numpy())
203
204 print('\nGround truth pose:')
205 print(get_ground_truth_pose(demo_sequence, args.source, args.target))
206
207
208def parse_args():
209 """Parses the command line arguments.
210
211 Returns:
212 The parsed command line arguments.
213 """
214 parser = argparse.ArgumentParser()
215 parser.add_argument('--source',
216 '-s',
217 type=int,
218 required=True,
219 help='Source point cloud index')
220 parser.add_argument('--target',
221 '-t',
222 type=int,
223 required=True,
224 help='Target point cloud index')
225 parser.add_argument('--device',
226 '-d',
227 default='cpu',
228 help='Device backend for the tensor',
229 choices=['cpu', 'cuda'])
230
231 return parser.parse_args()
232
233
234if __name__ == '__main__':
235 args = parse_args()
236 run_doppler_icp(args)
icp_registration.py#
1# ----------------------------------------------------------------------------
2# - CloudViewer: www.cloudViewer.org -
3# ----------------------------------------------------------------------------
4# Copyright (c) 2018-2024 www.cloudViewer.org
5# SPDX-License-Identifier: MIT
6# ----------------------------------------------------------------------------
7"""ICP (Iterative Closest Point) registration algorithm"""
8
9import cloudViewer as cv3d
10import numpy as np
11import copy
12
13
14def draw_registration_result(source, target, transformation):
15 source_temp = copy.deepcopy(source)
16 target_temp = copy.deepcopy(target)
17 source_temp.paint_uniform_color([1, 0.706, 0])
18 target_temp.paint_uniform_color([0, 0.651, 0.929])
19 source_temp.transform(transformation)
20 cv3d.visualization.draw([source_temp, target_temp])
21
22
23def point_to_point_icp(source, target, threshold, trans_init):
24 print("Apply point-to-point ICP")
25 reg_p2p = cv3d.pipelines.registration.registration_icp(
26 source, target, threshold, trans_init,
27 cv3d.pipelines.registration.TransformationEstimationPointToPoint())
28 print(reg_p2p)
29 print("Transformation is:")
30 print(reg_p2p.transformation, "\n")
31 draw_registration_result(source, target, reg_p2p.transformation)
32
33
34def point_to_plane_icp(source, target, threshold, trans_init):
35 print("Apply point-to-plane ICP")
36 reg_p2l = cv3d.pipelines.registration.registration_icp(
37 source, target, threshold, trans_init,
38 cv3d.pipelines.registration.TransformationEstimationPointToPlane())
39 print(reg_p2l)
40 print("Transformation is:")
41 print(reg_p2l.transformation, "\n")
42 draw_registration_result(source, target, reg_p2l.transformation)
43
44
45if __name__ == "__main__":
46 pcd_data = cv3d.data.DemoICPPointClouds()
47 source = cv3d.io.read_point_cloud(pcd_data.paths[0])
48 target = cv3d.io.read_point_cloud(pcd_data.paths[1])
49 threshold = 0.02
50 trans_init = np.asarray([[0.862, 0.011, -0.507, 0.5],
51 [-0.139, 0.967, -0.215, 0.7],
52 [0.487, 0.255, 0.835, -1.4], [0.0, 0.0, 0.0, 1.0]])
53 draw_registration_result(source, target, trans_init)
54
55 print("Initial alignment")
56 evaluation = cv3d.pipelines.registration.evaluate_registration(
57 source, target, threshold, trans_init)
58 print(evaluation, "\n")
59
60 point_to_point_icp(source, target, threshold, trans_init)
61 point_to_plane_icp(source, target, threshold, trans_init)
multiway_registration.py#
1# ----------------------------------------------------------------------------
2# - CloudViewer: www.cloudViewer.org -
3# ----------------------------------------------------------------------------
4# Copyright (c) 2018-2024 www.cloudViewer.org
5# SPDX-License-Identifier: MIT
6# ----------------------------------------------------------------------------
7"""Align multiple pieces of geometry in a global space"""
8
9import cloudViewer as cv3d
10import numpy as np
11
12
13def load_point_clouds(voxel_size=0.0):
14 pcd_data = cv3d.data.DemoICPPointClouds()
15 pcds = []
16 for i in range(3):
17 pcd = cv3d.io.read_point_cloud(pcd_data.paths[i])
18 pcd_down = pcd.voxel_down_sample(voxel_size=voxel_size)
19 pcds.append(pcd_down)
20 return pcds
21
22
23def pairwise_registration(source, target, max_correspondence_distance_coarse,
24 max_correspondence_distance_fine):
25 print("Apply point-to-plane ICP")
26 icp_coarse = cv3d.pipelines.registration.registration_icp(
27 source, target, max_correspondence_distance_coarse, np.identity(4),
28 cv3d.pipelines.registration.TransformationEstimationPointToPlane())
29 icp_fine = cv3d.pipelines.registration.registration_icp(
30 source, target, max_correspondence_distance_fine,
31 icp_coarse.transformation,
32 cv3d.pipelines.registration.TransformationEstimationPointToPlane())
33 transformation_icp = icp_fine.transformation
34 information_icp = cv3d.pipelines.registration.get_information_matrix_from_point_clouds(
35 source, target, max_correspondence_distance_fine,
36 icp_fine.transformation)
37 return transformation_icp, information_icp
38
39
40def full_registration(pcds, max_correspondence_distance_coarse,
41 max_correspondence_distance_fine):
42 pose_graph = cv3d.pipelines.registration.PoseGraph()
43 odometry = np.identity(4)
44 pose_graph.nodes.append(cv3d.pipelines.registration.PoseGraphNode(odometry))
45 n_pcds = len(pcds)
46 for source_id in range(n_pcds):
47 for target_id in range(source_id + 1, n_pcds):
48 transformation_icp, information_icp = pairwise_registration(
49 pcds[source_id], pcds[target_id],
50 max_correspondence_distance_coarse,
51 max_correspondence_distance_fine)
52 print("Build cv3d.pipelines.registration.PoseGraph")
53 if target_id == source_id + 1: # odometry case
54 odometry = np.dot(transformation_icp, odometry)
55 pose_graph.nodes.append(
56 cv3d.pipelines.registration.PoseGraphNode(
57 np.linalg.inv(odometry)))
58 pose_graph.edges.append(
59 cv3d.pipelines.registration.PoseGraphEdge(
60 source_id,
61 target_id,
62 transformation_icp,
63 information_icp,
64 uncertain=False))
65 else: # loop closure case
66 pose_graph.edges.append(
67 cv3d.pipelines.registration.PoseGraphEdge(
68 source_id,
69 target_id,
70 transformation_icp,
71 information_icp,
72 uncertain=True))
73 return pose_graph
74
75
76if __name__ == "__main__":
77 voxel_size = 0.02
78 pcds_down = load_point_clouds(voxel_size)
79 cv3d.visualization.draw(pcds_down)
80
81 print("Full registration ...")
82 max_correspondence_distance_coarse = voxel_size * 15
83 max_correspondence_distance_fine = voxel_size * 1.5
84 with cv3d.utility.VerbosityContextManager(
85 cv3d.utility.VerbosityLevel.Debug) as cm:
86 pose_graph = full_registration(pcds_down,
87 max_correspondence_distance_coarse,
88 max_correspondence_distance_fine)
89
90 print("Optimizing PoseGraph ...")
91 option = cv3d.pipelines.registration.GlobalOptimizationOption(
92 max_correspondence_distance=max_correspondence_distance_fine,
93 edge_prune_threshold=0.25,
94 reference_node=0)
95 with cv3d.utility.VerbosityContextManager(
96 cv3d.utility.VerbosityLevel.Debug) as cm:
97 cv3d.pipelines.registration.global_optimization(
98 pose_graph,
99 cv3d.pipelines.registration.GlobalOptimizationLevenbergMarquardt(),
100 cv3d.pipelines.registration.GlobalOptimizationConvergenceCriteria(),
101 option)
102
103 print("Transform points and display")
104 for point_id in range(len(pcds_down)):
105 print(pose_graph.nodes[point_id].pose)
106 pcds_down[point_id].transform(pose_graph.nodes[point_id].pose)
107 cv3d.visualization.draw(pcds_down)
pose_graph_optimization.py#
1# ----------------------------------------------------------------------------
2# - CloudViewer: www.cloudViewer.org -
3# ----------------------------------------------------------------------------
4# Copyright (c) 2018-2024 www.cloudViewer.org
5# SPDX-License-Identifier: MIT
6# ----------------------------------------------------------------------------
7
8import cloudViewer as cv3d
9import os
10
11if __name__ == "__main__":
12
13 cv3d.utility.set_verbosity_level(cv3d.utility.VerbosityLevel.Debug)
14
15 print("")
16 print(
17 "Parameters for cv3d.pipelines.registration.PoseGraph optimization ...")
18 method = cv3d.pipelines.registration.GlobalOptimizationLevenbergMarquardt()
19 criteria = cv3d.pipelines.registration.GlobalOptimizationConvergenceCriteria(
20 )
21 option = cv3d.pipelines.registration.GlobalOptimizationOption()
22 print("")
23 print(method)
24 print(criteria)
25 print(option)
26 print("")
27
28 print(
29 "Optimizing Fragment cv3d.pipelines.registration.PoseGraph using cloudViewer ..."
30 )
31
32 pose_graph_data = cv3d.data.DemoPoseGraphOptimization()
33 pose_graph_fragment = cv3d.io.read_pose_graph(
34 pose_graph_data.pose_graph_fragment_path)
35 print(pose_graph_fragment)
36 cv3d.pipelines.registration.global_optimization(pose_graph_fragment, method,
37 criteria, option)
38 cv3d.io.write_pose_graph(
39 os.path.join('pose_graph_example_fragment_optimized.json'),
40 pose_graph_fragment)
41 print("")
42
43 print(
44 "Optimizing Global cv3d.pipelines.registration.PoseGraph using cloudViewer ..."
45 )
46 pose_graph_global = cv3d.io.read_pose_graph(
47 pose_graph_data.pose_graph_global_path)
48 print(pose_graph_global)
49 cv3d.pipelines.registration.global_optimization(pose_graph_global, method,
50 criteria, option)
51 cv3d.io.write_pose_graph(
52 os.path.join('pose_graph_example_global_optimized.json'),
53 pose_graph_global)
registration_fgr.py#
1# ----------------------------------------------------------------------------
2# - CloudViewer: www.cloudViewer.org -
3# ----------------------------------------------------------------------------
4# Copyright (c) 2018-2024 www.cloudViewer.org
5# SPDX-License-Identifier: MIT
6# ----------------------------------------------------------------------------
7
8import cloudViewer as cv3d
9
10import argparse
11
12
13def preprocess_point_cloud(pcd, voxel_size):
14 pcd_down = pcd.voxel_down_sample(voxel_size)
15 pcd_down.estimate_normals(
16 cv3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size * 2.0,
17 max_nn=30))
18 pcd_fpfh = cv3d.pipelines.registration.compute_fpfh_feature(
19 pcd_down,
20 cv3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size * 5.0,
21 max_nn=100))
22 return (pcd_down, pcd_fpfh)
23
24
25if __name__ == '__main__':
26 pcd_data = cv3d.data.DemoICPPointClouds()
27 parser = argparse.ArgumentParser(
28 'Global point cloud registration example with RANSAC')
29 parser.add_argument('src',
30 type=str,
31 default=pcd_data.paths[0],
32 nargs='?',
33 help='path to src point cloud')
34 parser.add_argument('dst',
35 type=str,
36 default=pcd_data.paths[1],
37 nargs='?',
38 help='path to dst point cloud')
39 parser.add_argument('--voxel_size',
40 type=float,
41 default=0.05,
42 help='voxel size in meter used to downsample inputs')
43 parser.add_argument(
44 '--distance_multiplier',
45 type=float,
46 default=1.5,
47 help='multipler used to compute distance threshold'
48 'between correspondences.'
49 'Threshold is computed by voxel_size * distance_multiplier.')
50 parser.add_argument('--max_iterations',
51 type=int,
52 default=64,
53 help='number of max FGR iterations')
54 parser.add_argument(
55 '--max_tuples',
56 type=int,
57 default=1000,
58 help='max number of accepted tuples for correspondence filtering')
59
60 args = parser.parse_args()
61
62 voxel_size = args.voxel_size
63 distance_threshold = args.distance_multiplier * voxel_size
64
65 cv3d.utility.set_verbosity_level(cv3d.utility.VerbosityLevel.Debug)
66 print('Reading inputs')
67 src = cv3d.io.read_point_cloud(args.src)
68 dst = cv3d.io.read_point_cloud(args.dst)
69
70 print('Downsampling inputs')
71 src_down, src_fpfh = preprocess_point_cloud(src, voxel_size)
72 dst_down, dst_fpfh = preprocess_point_cloud(dst, voxel_size)
73
74 print('Running FGR')
75 result = cv3d.pipelines.registration.registration_fgr_based_on_feature_matching(
76 src_down, dst_down, src_fpfh, dst_fpfh,
77 cv3d.pipelines.registration.FastGlobalRegistrationOption(
78 maximum_correspondence_distance=distance_threshold,
79 iteration_number=args.max_iterations,
80 maximum_tuple_count=args.max_tuples))
81
82 src.paint_uniform_color([1, 0, 0])
83 dst.paint_uniform_color([0, 1, 0])
84 cv3d.visualization.draw([src.transform(result.transformation), dst])
registration_ransac.py#
1# ----------------------------------------------------------------------------
2# - CloudViewer: www.cloudViewer.org -
3# ----------------------------------------------------------------------------
4# Copyright (c) 2018-2024 www.cloudViewer.org
5# SPDX-License-Identifier: MIT
6# ----------------------------------------------------------------------------
7
8import cloudViewer as cv3d
9
10import numpy as np
11from copy import deepcopy
12import argparse
13
14
15def visualize_registration(src, dst, transformation=np.eye(4)):
16 src_trans = deepcopy(src)
17 src_trans.transform(transformation)
18 src_trans.paint_uniform_color([1, 0, 0])
19
20 dst_clone = deepcopy(dst)
21 dst_clone.paint_uniform_color([0, 1, 0])
22
23 cv3d.visualization.draw([src_trans, dst_clone])
24
25
26def preprocess_point_cloud(pcd, voxel_size):
27 pcd_down = pcd.voxel_down_sample(voxel_size)
28 pcd_down.estimate_normals(
29 cv3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size * 2.0,
30 max_nn=30))
31 pcd_fpfh = cv3d.pipelines.registration.compute_fpfh_feature(
32 pcd_down,
33 cv3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size * 5.0,
34 max_nn=100),
35 )
36 return (pcd_down, pcd_fpfh)
37
38
39if __name__ == "__main__":
40 pcd_data = cv3d.data.DemoICPPointClouds()
41
42 # yapf: disable
43 parser = argparse.ArgumentParser(
44 "Global point cloud registration example with RANSAC"
45 )
46 parser.add_argument(
47 "src", type=str, default=pcd_data.paths[0], nargs="?",
48 help="path to src point cloud",
49 )
50 parser.add_argument(
51 "dst", type=str, default=pcd_data.paths[1], nargs="?",
52 help="path to dst point cloud",
53 )
54 parser.add_argument(
55 "--voxel_size", type=float, default=0.05,
56 help="voxel size in meter used to downsample inputs",
57 )
58 parser.add_argument(
59 "--distance_multiplier", type=float, default=1.5,
60 help="multipler used to compute distance threshold"
61 "between correspondences."
62 "Threshold is computed by voxel_size * distance_multiplier.",
63 )
64 parser.add_argument(
65 "--max_iterations", type=int, default=100000,
66 help="number of max RANSAC iterations",
67 )
68 parser.add_argument(
69 "--confidence", type=float, default=0.999, help="RANSAC confidence"
70 )
71 parser.add_argument(
72 "--mutual_filter", action="store_true",
73 help="whether to use mutual filter for putative correspondences",
74 )
75 parser.add_argument(
76 "--method", choices=["from_features", "from_correspondences"], default="from_correspondences"
77 )
78 # yapf: enable
79
80 args = parser.parse_args()
81
82 voxel_size = args.voxel_size
83 distance_threshold = args.distance_multiplier * voxel_size
84 cv3d.utility.set_verbosity_level(cv3d.utility.VerbosityLevel.Debug)
85
86 print("Reading inputs")
87 src = cv3d.io.read_point_cloud(args.src)
88 dst = cv3d.io.read_point_cloud(args.dst)
89
90 print("Downsampling inputs")
91 src_down, src_fpfh = preprocess_point_cloud(src, voxel_size)
92 dst_down, dst_fpfh = preprocess_point_cloud(dst, voxel_size)
93
94 if args.method == "from_features":
95 print("Running RANSAC from features")
96 result = cv3d.pipelines.registration.registration_ransac_based_on_feature_matching(
97 src_down,
98 dst_down,
99 src_fpfh,
100 dst_fpfh,
101 mutual_filter=args.mutual_filter,
102 max_correspondence_distance=distance_threshold,
103 estimation_method=cv3d.pipelines.registration.
104 TransformationEstimationPointToPoint(False),
105 ransac_n=3,
106 checkers=[
107 cv3d.pipelines.registration.
108 CorrespondenceCheckerBasedOnEdgeLength(0.9),
109 cv3d.pipelines.registration.
110 CorrespondenceCheckerBasedOnDistance(distance_threshold),
111 ],
112 criteria=cv3d.pipelines.registration.RANSACConvergenceCriteria(
113 args.max_iterations, args.confidence),
114 )
115 visualize_registration(src, dst, result.transformation)
116
117 elif args.method == "from_correspondences":
118 print("Running RANSAC from correspondences")
119 # Mimic importing customized external features (e.g. learned FCGF features) in numpy
120 # shape: (feature_dim, num_features)
121 src_fpfh_np = np.asarray(src_fpfh.data).copy()
122 dst_fpfh_np = np.asarray(dst_fpfh.data).copy()
123
124 src_fpfh_import = cv3d.pipelines.registration.Feature()
125 src_fpfh_import.data = src_fpfh_np
126
127 dst_fpfh_import = cv3d.pipelines.registration.Feature()
128 dst_fpfh_import.data = dst_fpfh_np
129
130 corres = cv3d.pipelines.registration.correspondences_from_features(
131 src_fpfh_import, dst_fpfh_import, args.mutual_filter)
132 result = cv3d.pipelines.registration.registration_ransac_based_on_correspondence(
133 src_down,
134 dst_down,
135 corres,
136 max_correspondence_distance=distance_threshold,
137 estimation_method=cv3d.pipelines.registration.
138 TransformationEstimationPointToPoint(False),
139 ransac_n=3,
140 checkers=[
141 cv3d.pipelines.registration.
142 CorrespondenceCheckerBasedOnEdgeLength(0.9),
143 cv3d.pipelines.registration.
144 CorrespondenceCheckerBasedOnDistance(distance_threshold),
145 ],
146 criteria=cv3d.pipelines.registration.RANSACConvergenceCriteria(
147 args.max_iterations, args.confidence),
148 )
149 visualize_registration(src, dst, result.transformation)
rgbd_integration_uniform.py#
1# ----------------------------------------------------------------------------
2# - CloudViewer: www.cloudViewer.org -
3# ----------------------------------------------------------------------------
4# Copyright (c) 2018-2024 www.cloudViewer.org
5# SPDX-License-Identifier: MIT
6# ----------------------------------------------------------------------------
7
8# examples/Python/Advanced/rgbd_integration_uniform.py
9
10import cloudViewer as cv3d
11import numpy as np
12
13import os, sys
14
15pyexample_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
16sys.path.append(pyexample_path)
17
18from cloudViewer_example import read_trajectory
19
20if __name__ == "__main__":
21 rgbd_data = cv3d.data.SampleRedwoodRGBDImages()
22 camera_poses = read_trajectory(rgbd_data.odometry_log_path)
23 camera_intrinsics = cv3d.camera.PinholeCameraIntrinsic(
24 cv3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault)
25 volume = cv3d.pipelines.integration.UniformTSDFVolume(
26 length=4.0,
27 resolution=512,
28 sdf_trunc=0.04,
29 color_type=cv3d.pipelines.integration.TSDFVolumeColorType.RGB8,
30 )
31
32 for i in range(len(camera_poses)):
33 print("Integrate {:d}-th image into the volume.".format(i))
34 color = cv3d.io.read_image(rgbd_data.color_paths[i])
35 depth = cv3d.io.read_image(rgbd_data.depth_paths[i])
36
37 rgbd = cv3d.geometry.RGBDImage.create_from_color_and_depth(
38 color, depth, depth_trunc=4.0, convert_rgb_to_intensity=False)
39 volume.integrate(
40 rgbd,
41 camera_intrinsics,
42 np.linalg.inv(camera_poses[i].pose),
43 )
44
45 print("Extract triangle mesh")
46 mesh = volume.extract_triangle_mesh()
47 mesh.compute_vertex_normals()
48 cv3d.visualization.draw_geometries([mesh])
49
50 print("Extract voxel-aligned debugging point cloud")
51 voxel_pcd = volume.extract_voxel_point_cloud()
52 cv3d.visualization.draw_geometries([voxel_pcd])
53
54 print("Extract voxel-aligned debugging voxel grid")
55 voxel_grid = volume.extract_voxel_grid()
56 cv3d.visualization.draw_geometries([voxel_grid])
57
58 print("Extract point cloud")
59 pcd = volume.extract_point_cloud()
60 cv3d.visualization.draw_geometries([pcd])
rgbd_odometry.py#
1# ----------------------------------------------------------------------------
2# - CloudViewer: www.cloudViewer.org -
3# ----------------------------------------------------------------------------
4# Copyright (c) 2018-2024 www.cloudViewer.org
5# SPDX-License-Identifier: MIT
6# ----------------------------------------------------------------------------
7
8import cloudViewer as cv3d
9import numpy as np
10
11if __name__ == "__main__":
12 pinhole_camera_intrinsic = cv3d.camera.PinholeCameraIntrinsic(
13 cv3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault)
14 rgbd_data = cv3d.data.SampleRedwoodRGBDImages()
15 source_color = cv3d.io.read_image(rgbd_data.color_paths[0])
16 source_depth = cv3d.io.read_image(rgbd_data.depth_paths[0])
17 target_color = cv3d.io.read_image(rgbd_data.color_paths[1])
18 target_depth = cv3d.io.read_image(rgbd_data.depth_paths[1])
19
20 source_rgbd_image = cv3d.geometry.RGBDImage.create_from_color_and_depth(
21 source_color, source_depth)
22 target_rgbd_image = cv3d.geometry.RGBDImage.create_from_color_and_depth(
23 target_color, target_depth)
24 target_pcd = cv3d.geometry.ccPointCloud.create_from_rgbd_image(
25 target_rgbd_image, pinhole_camera_intrinsic)
26
27 option = cv3d.pipelines.odometry.OdometryOption()
28 odo_init = np.identity(4)
29 print(option)
30
31 [success_color_term, trans_color_term,
32 info] = cv3d.pipelines.odometry.compute_rgbd_odometry(
33 source_rgbd_image, target_rgbd_image,
34 pinhole_camera_intrinsic, odo_init,
35 cv3d.pipelines.odometry.RGBDOdometryJacobianFromColorTerm(), option)
36 [success_hybrid_term, trans_hybrid_term,
37 info] = cv3d.pipelines.odometry.compute_rgbd_odometry(
38 source_rgbd_image, target_rgbd_image,
39 pinhole_camera_intrinsic, odo_init,
40 cv3d.pipelines.odometry.RGBDOdometryJacobianFromHybridTerm(), option)
41
42 if success_color_term:
43 print("Using RGB-D Odometry")
44 print(trans_color_term)
45 source_pcd_color_term = cv3d.geometry.ccPointCloud.create_from_rgbd_image(
46 source_rgbd_image, pinhole_camera_intrinsic)
47 source_pcd_color_term.transform(trans_color_term)
48 cv3d.visualization.draw([target_pcd, source_pcd_color_term])
49
50 if success_hybrid_term:
51 print("Using Hybrid RGB-D Odometry")
52 print(trans_hybrid_term)
53 source_pcd_hybrid_term = cv3d.geometry.ccPointCloud.create_from_rgbd_image(
54 source_rgbd_image, pinhole_camera_intrinsic)
55 source_pcd_hybrid_term.transform(trans_hybrid_term)
56 cv3d.visualization.draw([target_pcd, source_pcd_hybrid_term])
robust_icp.py#
1# ----------------------------------------------------------------------------
2# - CloudViewer: www.cloudViewer.org -
3# ----------------------------------------------------------------------------
4# Copyright (c) 2018-2024 www.cloudViewer.org
5# SPDX-License-Identifier: MIT
6# ----------------------------------------------------------------------------
7
8import cloudViewer as cv3d
9import numpy as np
10import copy
11
12
13def draw_registration_result(source, target, transformation):
14 source_temp = copy.deepcopy(source)
15 target_temp = copy.deepcopy(target)
16 source_temp.paint_uniform_color([1, 0.706, 0])
17 target_temp.paint_uniform_color([0, 0.651, 0.929])
18 source_temp.transform(transformation)
19 cv3d.visualization.draw([source_temp, target_temp])
20
21
22def apply_noise(pcd, mu, sigma):
23 noisy_pcd = copy.deepcopy(pcd)
24 points = np.asarray(noisy_pcd.points())
25 points += np.random.normal(mu, sigma, size=points.shape)
26 noisy_pcd.set_points(cv3d.utility.Vector3dVector(points))
27 return noisy_pcd
28
29
30if __name__ == "__main__":
31 pcd_data = cv3d.data.DemoICPPointClouds()
32 source = cv3d.io.read_point_cloud(pcd_data.paths[0])
33 target = cv3d.io.read_point_cloud(pcd_data.paths[1])
34 trans_init = np.asarray([[0.862, 0.011, -0.507, 0.5],
35 [-0.139, 0.967, -0.215, 0.7],
36 [0.487, 0.255, 0.835, -1.4], [0.0, 0.0, 0.0, 1.0]])
37
38 # Mean and standard deviation.
39 mu, sigma = 0, 0.1
40 source_noisy = apply_noise(source, mu, sigma)
41
42 print("Displaying source point cloud + noise:")
43 cv3d.visualization.draw([source_noisy])
44
45 print(
46 "Displaying original source and target point cloud with initial transformation:"
47 )
48 draw_registration_result(source, target, trans_init)
49
50 threshold = 1.0
51 print("Using the noisy source pointcloud to perform robust ICP.\n")
52 print("Robust point-to-plane ICP, threshold={}:".format(threshold))
53 loss = cv3d.pipelines.registration.TukeyLoss(k=sigma)
54 print("Using robust loss:", loss)
55 p2l = cv3d.pipelines.registration.TransformationEstimationPointToPlane(loss)
56 reg_p2l = cv3d.pipelines.registration.registration_icp(
57 source_noisy, target, threshold, trans_init, p2l)
58 print(reg_p2l)
59 print("Transformation is:")
60 print(reg_p2l.transformation)
61 draw_registration_result(source, target, reg_p2l.transformation)