Pipelines#
colored_icp_registration.py#
1# ----------------------------------------------------------------------------
2# - Open3D: www.open3d.org -
3# ----------------------------------------------------------------------------
4# Copyright (c) 2018-2023 www.open3d.org
5# SPDX-License-Identifier: MIT
6# ----------------------------------------------------------------------------
7"""ICP variant that uses both geometry and color for registration"""
8
9import open3d as o3d
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 o3d.visualization.draw([source_temp, target])
18
19
20print("Load two point clouds and show initial pose ...")
21ply_data = o3d.data.DemoColoredICPPointClouds()
22source = o3d.io.read_point_cloud(ply_data.paths[0])
23target = o3d.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 o3d.geometry.KDTreeSearchParamHybrid(radius=radius * 2, max_nn=30))
51 target_down.estimate_normals(
52 o3d.geometry.KDTreeSearchParamHybrid(radius=radius * 2, max_nn=30))
53
54 print("3. Applying colored point cloud registration")
55 result_icp = o3d.pipelines.registration.registration_colored_icp(
56 source_down, target_down, radius, current_transformation,
57 o3d.pipelines.registration.TransformationEstimationForColoredICP(),
58 o3d.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# - Open3D: www.open3d.org -
3# ----------------------------------------------------------------------------
4# Copyright (c) 2018-2023 www.open3d.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 open3d as o3d
26import open3d.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 = o3d.core.float32
130 device = o3d.core.Device('CUDA:0' if args.device == 'cuda' else 'CPU:0')
131
132 # Load the point clouds.
133 demo_sequence = o3d.data.DemoDopplerICPSequence()
134 source = o3d.t.io.read_point_cloud(demo_sequence.paths[args.source])
135 target = o3d.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 = o3d.core.Tensor(np.eye(4), device=device)
150 transform_vehicle_to_sensor = o3d.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'] = o3d.core.Tensor(directions, dtype, device)
161
162 # Setup robust kernels.
163 kernel = o3d_reg.robust_kernel.RobustKernel(o3d_reg.robust_kernel.TukeyLoss,
164 scaling_parameter=0.5)
165
166 # Setup convergence criteria.
167 criteria = o3d_reg.ICPConvergenceCriteria(relative_fitness=1e-6,
168 relative_rmse=1e-6,
169 max_iteration=200)
170
171 # Setup transformation estimator.
172 estimator_p2l = o3d_reg.TransformationEstimationPointToPlane(kernel)
173 estimator_dicp = o3d_reg.TransformationEstimationForDopplerICP(
174 period=period * (args.target - args.source),
175 lambda_doppler=0.01,
176 reject_dynamic_outliers=False,
177 doppler_outlier_threshold=2.0,
178 outlier_rejection_min_iteration=2,
179 geometric_robust_loss_min_iteration=0,
180 doppler_robust_loss_min_iteration=2,
181 goemetric_kernel=kernel,
182 doppler_kernel=kernel,
183 transform_vehicle_to_sensor=transform_vehicle_to_sensor)
184
185 # Run Doppler ICP and point-to-plane ICP registration for comparison.
186 max_neighbor_distance = 0.3
187 results = [
188 o3d_reg.icp(source_in_V, target_in_V, max_neighbor_distance,
189 init_transform, estimator, criteria)
190 for estimator in [estimator_p2l, estimator_dicp]
191 ]
192
193 # Display the poses.
194 np.set_printoptions(suppress=True, precision=4)
195 print('Estimated pose from Point-to-Plane ICP [%s iterations]:' %
196 results[0].num_iterations)
197 print(results[0].transformation.numpy())
198
199 print('\nEstimated pose from Doppler ICP [%s iterations]:' %
200 results[1].num_iterations)
201 print(results[1].transformation.numpy())
202
203 print('\nGround truth pose:')
204 print(get_ground_truth_pose(demo_sequence, args.source, args.target))
205
206
207def parse_args():
208 """Parses the command line arguments.
209
210 Returns:
211 The parsed command line arguments.
212 """
213 parser = argparse.ArgumentParser()
214 parser.add_argument('--source',
215 '-s',
216 type=int,
217 required=True,
218 help='Source point cloud index')
219 parser.add_argument('--target',
220 '-t',
221 type=int,
222 required=True,
223 help='Target point cloud index')
224 parser.add_argument('--device',
225 '-d',
226 default='cpu',
227 help='Device backend for the tensor',
228 choices=['cpu', 'cuda'])
229
230 return parser.parse_args()
231
232
233if __name__ == '__main__':
234 args = parse_args()
235 run_doppler_icp(args)
icp_registration.py#
1# ----------------------------------------------------------------------------
2# - Open3D: www.open3d.org -
3# ----------------------------------------------------------------------------
4# Copyright (c) 2018-2023 www.open3d.org
5# SPDX-License-Identifier: MIT
6# ----------------------------------------------------------------------------
7"""ICP (Iterative Closest Point) registration algorithm"""
8
9import open3d as o3d
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 o3d.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 = o3d.pipelines.registration.registration_icp(
26 source, target, threshold, trans_init,
27 o3d.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 = o3d.pipelines.registration.registration_icp(
37 source, target, threshold, trans_init,
38 o3d.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 = o3d.data.DemoICPPointClouds()
47 source = o3d.io.read_point_cloud(pcd_data.paths[0])
48 target = o3d.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 = o3d.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# - Open3D: www.open3d.org -
3# ----------------------------------------------------------------------------
4# Copyright (c) 2018-2023 www.open3d.org
5# SPDX-License-Identifier: MIT
6# ----------------------------------------------------------------------------
7"""Align multiple pieces of geometry in a global space"""
8
9import open3d as o3d
10import numpy as np
11
12
13def load_point_clouds(voxel_size=0.0):
14 pcd_data = o3d.data.DemoICPPointClouds()
15 pcds = []
16 for i in range(3):
17 pcd = o3d.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 = o3d.pipelines.registration.registration_icp(
27 source, target, max_correspondence_distance_coarse, np.identity(4),
28 o3d.pipelines.registration.TransformationEstimationPointToPlane())
29 icp_fine = o3d.pipelines.registration.registration_icp(
30 source, target, max_correspondence_distance_fine,
31 icp_coarse.transformation,
32 o3d.pipelines.registration.TransformationEstimationPointToPlane())
33 transformation_icp = icp_fine.transformation
34 information_icp = o3d.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 = o3d.pipelines.registration.PoseGraph()
43 odometry = np.identity(4)
44 pose_graph.nodes.append(o3d.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 o3d.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 o3d.pipelines.registration.PoseGraphNode(
57 np.linalg.inv(odometry)))
58 pose_graph.edges.append(
59 o3d.pipelines.registration.PoseGraphEdge(source_id,
60 target_id,
61 transformation_icp,
62 information_icp,
63 uncertain=False))
64 else: # loop closure case
65 pose_graph.edges.append(
66 o3d.pipelines.registration.PoseGraphEdge(source_id,
67 target_id,
68 transformation_icp,
69 information_icp,
70 uncertain=True))
71 return pose_graph
72
73
74if __name__ == "__main__":
75 voxel_size = 0.02
76 pcds_down = load_point_clouds(voxel_size)
77 o3d.visualization.draw(pcds_down)
78
79 print("Full registration ...")
80 max_correspondence_distance_coarse = voxel_size * 15
81 max_correspondence_distance_fine = voxel_size * 1.5
82 with o3d.utility.VerbosityContextManager(
83 o3d.utility.VerbosityLevel.Debug) as cm:
84 pose_graph = full_registration(pcds_down,
85 max_correspondence_distance_coarse,
86 max_correspondence_distance_fine)
87
88 print("Optimizing PoseGraph ...")
89 option = o3d.pipelines.registration.GlobalOptimizationOption(
90 max_correspondence_distance=max_correspondence_distance_fine,
91 edge_prune_threshold=0.25,
92 reference_node=0)
93 with o3d.utility.VerbosityContextManager(
94 o3d.utility.VerbosityLevel.Debug) as cm:
95 o3d.pipelines.registration.global_optimization(
96 pose_graph,
97 o3d.pipelines.registration.GlobalOptimizationLevenbergMarquardt(),
98 o3d.pipelines.registration.GlobalOptimizationConvergenceCriteria(),
99 option)
100
101 print("Transform points and display")
102 for point_id in range(len(pcds_down)):
103 print(pose_graph.nodes[point_id].pose)
104 pcds_down[point_id].transform(pose_graph.nodes[point_id].pose)
105 o3d.visualization.draw(pcds_down)
pose_graph_optimization.py#
1# ----------------------------------------------------------------------------
2# - Open3D: www.open3d.org -
3# ----------------------------------------------------------------------------
4# Copyright (c) 2018-2023 www.open3d.org
5# SPDX-License-Identifier: MIT
6# ----------------------------------------------------------------------------
7
8import open3d as o3d
9import numpy as np
10import os
11
12if __name__ == "__main__":
13
14 o3d.utility.set_verbosity_level(o3d.utility.VerbosityLevel.Debug)
15
16 print("")
17 print(
18 "Parameters for o3d.pipelines.registration.PoseGraph optimization ...")
19 method = o3d.pipelines.registration.GlobalOptimizationLevenbergMarquardt()
20 criteria = o3d.pipelines.registration.GlobalOptimizationConvergenceCriteria(
21 )
22 option = o3d.pipelines.registration.GlobalOptimizationOption()
23 print("")
24 print(method)
25 print(criteria)
26 print(option)
27 print("")
28
29 print(
30 "Optimizing Fragment o3d.pipelines.registration.PoseGraph using open3d ..."
31 )
32
33 pose_graph_data = o3d.data.DemoPoseGraphOptimization()
34 pose_graph_fragment = o3d.io.read_pose_graph(
35 pose_graph_data.pose_graph_fragment_path)
36 print(pose_graph_fragment)
37 o3d.pipelines.registration.global_optimization(pose_graph_fragment, method,
38 criteria, option)
39 o3d.io.write_pose_graph(
40 os.path.join('pose_graph_example_fragment_optimized.json'),
41 pose_graph_fragment)
42 print("")
43
44 print(
45 "Optimizing Global o3d.pipelines.registration.PoseGraph using open3d ..."
46 )
47 pose_graph_global = o3d.io.read_pose_graph(
48 pose_graph_data.pose_graph_global_path)
49 print(pose_graph_global)
50 o3d.pipelines.registration.global_optimization(pose_graph_global, method,
51 criteria, option)
52 o3d.io.write_pose_graph(
53 os.path.join('pose_graph_example_global_optimized.json'),
54 pose_graph_global)
registration_fgr.py#
1# ----------------------------------------------------------------------------
2# - Open3D: www.open3d.org -
3# ----------------------------------------------------------------------------
4# Copyright (c) 2018-2023 www.open3d.org
5# SPDX-License-Identifier: MIT
6# ----------------------------------------------------------------------------
7
8import open3d as o3d
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 o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size * 2.0,
17 max_nn=30))
18 pcd_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
19 pcd_down,
20 o3d.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 = o3d.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 o3d.utility.set_verbosity_level(o3d.utility.VerbosityLevel.Debug)
66 print('Reading inputs')
67 src = o3d.io.read_point_cloud(args.src)
68 dst = o3d.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 = o3d.pipelines.registration.registration_fgr_based_on_feature_matching(
76 src_down, dst_down, src_fpfh, dst_fpfh,
77 o3d.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 o3d.visualization.draw([src.transform(result.transformation), dst])
registration_ransac.py#
1# ----------------------------------------------------------------------------
2# - Open3D: www.open3d.org -
3# ----------------------------------------------------------------------------
4# Copyright (c) 2018-2023 www.open3d.org
5# SPDX-License-Identifier: MIT
6# ----------------------------------------------------------------------------
7
8import open3d as o3d
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 o3d.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 o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size * 2.0,
30 max_nn=30))
31 pcd_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
32 pcd_down,
33 o3d.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 = o3d.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 o3d.utility.set_verbosity_level(o3d.utility.VerbosityLevel.Debug)
85
86 print("Reading inputs")
87 src = o3d.io.read_point_cloud(args.src)
88 dst = o3d.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 = o3d.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=o3d.pipelines.registration.
104 TransformationEstimationPointToPoint(False),
105 ransac_n=3,
106 checkers=[
107 o3d.pipelines.registration.
108 CorrespondenceCheckerBasedOnEdgeLength(0.9),
109 o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(
110 distance_threshold),
111 ],
112 criteria=o3d.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 = o3d.pipelines.registration.Feature()
125 src_fpfh_import.data = src_fpfh_np
126
127 dst_fpfh_import = o3d.pipelines.registration.Feature()
128 dst_fpfh_import.data = dst_fpfh_np
129
130 corres = o3d.pipelines.registration.correspondences_from_features(
131 src_fpfh_import, dst_fpfh_import, args.mutual_filter)
132 result = o3d.pipelines.registration.registration_ransac_based_on_correspondence(
133 src_down,
134 dst_down,
135 corres,
136 max_correspondence_distance=distance_threshold,
137 estimation_method=o3d.pipelines.registration.
138 TransformationEstimationPointToPoint(False),
139 ransac_n=3,
140 checkers=[
141 o3d.pipelines.registration.
142 CorrespondenceCheckerBasedOnEdgeLength(0.9),
143 o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(
144 distance_threshold),
145 ],
146 criteria=o3d.pipelines.registration.RANSACConvergenceCriteria(
147 args.max_iterations, args.confidence),
148 )
149 visualize_registration(src, dst, result.transformation)
rgbd_integration_uniform.py#
1# ----------------------------------------------------------------------------
2# - Open3D: www.open3d.org -
3# ----------------------------------------------------------------------------
4# Copyright (c) 2018-2023 www.open3d.org
5# SPDX-License-Identifier: MIT
6# ----------------------------------------------------------------------------
7
8import open3d as o3d
9import numpy as np
10
11import os, sys
12
13pyexample_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
14sys.path.append(pyexample_path)
15
16from open3d_example import read_trajectory
17
18if __name__ == "__main__":
19 rgbd_data = o3d.data.SampleRedwoodRGBDImages()
20 camera_poses = read_trajectory(rgbd_data.odometry_log_path)
21 camera_intrinsics = o3d.camera.PinholeCameraIntrinsic(
22 o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault)
23 volume = o3d.pipelines.integration.UniformTSDFVolume(
24 length=4.0,
25 resolution=512,
26 sdf_trunc=0.04,
27 color_type=o3d.pipelines.integration.TSDFVolumeColorType.RGB8,
28 )
29
30 for i in range(len(camera_poses)):
31 print("Integrate {:d}-th image into the volume.".format(i))
32 color = o3d.io.read_image(rgbd_data.color_paths[i])
33 depth = o3d.io.read_image(rgbd_data.depth_paths[i])
34
35 rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(
36 color, depth, depth_trunc=4.0, convert_rgb_to_intensity=False)
37 volume.integrate(
38 rgbd,
39 camera_intrinsics,
40 np.linalg.inv(camera_poses[i].pose),
41 )
42
43 print("Extract triangle mesh")
44 mesh = volume.extract_triangle_mesh()
45 mesh.compute_vertex_normals()
46 o3d.visualization.draw_geometries([mesh])
47
48 print("Extract voxel-aligned debugging point cloud")
49 voxel_pcd = volume.extract_voxel_point_cloud()
50 o3d.visualization.draw_geometries([voxel_pcd])
51
52 print("Extract voxel-aligned debugging voxel grid")
53 voxel_grid = volume.extract_voxel_grid()
54 # o3d.visualization.draw_geometries([voxel_grid])
55
56 # print("Extract point cloud")
57 # pcd = volume.extract_point_cloud()
58 # o3d.visualization.draw_geometries([pcd])
rgbd_odometry.py#
1# ----------------------------------------------------------------------------
2# - Open3D: www.open3d.org -
3# ----------------------------------------------------------------------------
4# Copyright (c) 2018-2023 www.open3d.org
5# SPDX-License-Identifier: MIT
6# ----------------------------------------------------------------------------
7"""Find camera movement between two consecutive RGBD image pairs"""
8
9import open3d as o3d
10import numpy as np
11
12if __name__ == "__main__":
13 pinhole_camera_intrinsic = o3d.camera.PinholeCameraIntrinsic(
14 o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault)
15 rgbd_data = o3d.data.SampleRedwoodRGBDImages()
16 source_color = o3d.io.read_image(rgbd_data.color_paths[0])
17 source_depth = o3d.io.read_image(rgbd_data.depth_paths[0])
18 target_color = o3d.io.read_image(rgbd_data.color_paths[1])
19 target_depth = o3d.io.read_image(rgbd_data.depth_paths[1])
20
21 source_rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
22 source_color, source_depth)
23 target_rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
24 target_color, target_depth)
25 target_pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
26 target_rgbd_image, pinhole_camera_intrinsic)
27
28 option = o3d.pipelines.odometry.OdometryOption()
29 odo_init = np.identity(4)
30 print(option)
31
32 [success_color_term, trans_color_term,
33 info] = o3d.pipelines.odometry.compute_rgbd_odometry(
34 source_rgbd_image, target_rgbd_image,
35 pinhole_camera_intrinsic, odo_init,
36 o3d.pipelines.odometry.RGBDOdometryJacobianFromColorTerm(), option)
37 [success_hybrid_term, trans_hybrid_term,
38 info] = o3d.pipelines.odometry.compute_rgbd_odometry(
39 source_rgbd_image, target_rgbd_image,
40 pinhole_camera_intrinsic, odo_init,
41 o3d.pipelines.odometry.RGBDOdometryJacobianFromHybridTerm(), option)
42
43 if success_color_term:
44 print("Using RGB-D Odometry")
45 print(trans_color_term)
46 source_pcd_color_term = o3d.geometry.PointCloud.create_from_rgbd_image(
47 source_rgbd_image, pinhole_camera_intrinsic)
48 source_pcd_color_term.transform(trans_color_term)
49 o3d.visualization.draw([target_pcd, source_pcd_color_term])
50
51 if success_hybrid_term:
52 print("Using Hybrid RGB-D Odometry")
53 print(trans_hybrid_term)
54 source_pcd_hybrid_term = o3d.geometry.PointCloud.create_from_rgbd_image(
55 source_rgbd_image, pinhole_camera_intrinsic)
56 source_pcd_hybrid_term.transform(trans_hybrid_term)
57 o3d.visualization.draw([target_pcd, source_pcd_hybrid_term])
robust_icp.py#
1# ----------------------------------------------------------------------------
2# - Open3D: www.open3d.org -
3# ----------------------------------------------------------------------------
4# Copyright (c) 2018-2023 www.open3d.org
5# SPDX-License-Identifier: MIT
6# ----------------------------------------------------------------------------
7"""Outlier rejection using robust kernels for ICP"""
8
9import open3d as o3d
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 o3d.visualization.draw([source_temp, target_temp])
21
22
23def apply_noise(pcd, mu, sigma):
24 noisy_pcd = copy.deepcopy(pcd)
25 points = np.asarray(noisy_pcd.points)
26 points += np.random.normal(mu, sigma, size=points.shape)
27 noisy_pcd.points = o3d.utility.Vector3dVector(points)
28 return noisy_pcd
29
30
31if __name__ == "__main__":
32 pcd_data = o3d.data.DemoICPPointClouds()
33 source = o3d.io.read_point_cloud(pcd_data.paths[0])
34 target = o3d.io.read_point_cloud(pcd_data.paths[1])
35 trans_init = np.asarray([[0.862, 0.011, -0.507, 0.5],
36 [-0.139, 0.967, -0.215, 0.7],
37 [0.487, 0.255, 0.835, -1.4], [0.0, 0.0, 0.0, 1.0]])
38
39 # Mean and standard deviation.
40 mu, sigma = 0, 0.1
41 source_noisy = apply_noise(source, mu, sigma)
42
43 print("Displaying source point cloud + noise:")
44 o3d.visualization.draw([source_noisy])
45
46 print(
47 "Displaying original source and target point cloud with initial transformation:"
48 )
49 draw_registration_result(source, target, trans_init)
50
51 threshold = 1.0
52 print("Using the noisy source pointcloud to perform robust ICP.\n")
53 print("Robust point-to-plane ICP, threshold={}:".format(threshold))
54 loss = o3d.pipelines.registration.TukeyLoss(k=sigma)
55 print("Using robust loss:", loss)
56 p2l = o3d.pipelines.registration.TransformationEstimationPointToPlane(loss)
57 reg_p2l = o3d.pipelines.registration.registration_icp(
58 source_noisy, target, threshold, trans_init, p2l)
59 print(reg_p2l)
60 print("Transformation is:")
61 print(reg_p2l.transformation)
62 draw_registration_result(source, target, reg_p2l.transformation)