Pipelines¶
colored_pointcloud_registration.py¶
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 | # ----------------------------------------------------------------------------
# - Open3D: www.open3d.org -
# ----------------------------------------------------------------------------
# The MIT License (MIT)
#
# Copyright (c) 2018-2021 www.open3d.org
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in
# all copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
# FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
# IN THE SOFTWARE.
# ----------------------------------------------------------------------------
"""ICP variant that uses both geometry and color for registration"""
import open3d as o3d
import numpy as np
import copy
def draw_registration_result(source, target, transformation):
source_temp = copy.deepcopy(source)
source_temp.transform(transformation)
o3d.visualization.draw([source_temp, target])
print("Load two point clouds and show initial pose ...")
ply_data = o3d.data.DemoColoredICPPointClouds()
source = o3d.io.read_point_cloud(ply_data.paths[0])
target = o3d.io.read_point_cloud(ply_data.paths[1])
if __name__ == "__main__":
# Draw initial alignment.
current_transformation = np.identity(4)
draw_registration_result(source, target, current_transformation)
# Colored pointcloud registration.
# This is implementation of following paper:
# J. Park, Q.-Y. Zhou, V. Koltun,
# Colored Point Cloud Registration Revisited, ICCV 2017.
voxel_radius = [0.04, 0.02, 0.01]
max_iter = [50, 30, 14]
current_transformation = np.identity(4)
print("Colored point cloud registration ...\n")
for scale in range(3):
iter = max_iter[scale]
radius = voxel_radius[scale]
print([iter, radius, scale])
print("1. Downsample with a voxel size %.2f" % radius)
source_down = source.voxel_down_sample(radius)
target_down = target.voxel_down_sample(radius)
print("2. Estimate normal")
source_down.estimate_normals(
o3d.geometry.KDTreeSearchParamHybrid(radius=radius * 2, max_nn=30))
target_down.estimate_normals(
o3d.geometry.KDTreeSearchParamHybrid(radius=radius * 2, max_nn=30))
print("3. Applying colored point cloud registration")
result_icp = o3d.pipelines.registration.registration_colored_icp(
source_down, target_down, radius, current_transformation,
o3d.pipelines.registration.TransformationEstimationForColoredICP(),
o3d.pipelines.registration.ICPConvergenceCriteria(
relative_fitness=1e-6, relative_rmse=1e-6, max_iteration=iter))
current_transformation = result_icp.transformation
print(result_icp, "\n")
draw_registration_result(source, target, result_icp.transformation)
|
icp_registration.py¶
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 | # ----------------------------------------------------------------------------
# - Open3D: www.open3d.org -
# ----------------------------------------------------------------------------
# The MIT License (MIT)
#
# Copyright (c) 2018-2021 www.open3d.org
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in
# all copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
# FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
# IN THE SOFTWARE.
# ----------------------------------------------------------------------------
"""ICP (Iterative Closest Point) registration algorithm"""
import open3d as o3d
import numpy as np
import copy
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([source_temp, target_temp])
def point_to_point_icp(source, target, threshold, trans_init):
print("Apply point-to-point ICP")
reg_p2p = o3d.pipelines.registration.registration_icp(
source, target, threshold, trans_init,
o3d.pipelines.registration.TransformationEstimationPointToPoint())
print(reg_p2p)
print("Transformation is:")
print(reg_p2p.transformation, "\n")
draw_registration_result(source, target, reg_p2p.transformation)
def point_to_plane_icp(source, target, threshold, trans_init):
print("Apply point-to-plane ICP")
reg_p2l = o3d.pipelines.registration.registration_icp(
source, target, threshold, trans_init,
o3d.pipelines.registration.TransformationEstimationPointToPlane())
print(reg_p2l)
print("Transformation is:")
print(reg_p2l.transformation, "\n")
draw_registration_result(source, target, reg_p2l.transformation)
if __name__ == "__main__":
pcd_data = o3d.data.DemoICPPointClouds()
source = o3d.io.read_point_cloud(pcd_data.paths[0])
target = o3d.io.read_point_cloud(pcd_data.paths[1])
threshold = 0.02
trans_init = np.asarray([[0.862, 0.011, -0.507, 0.5],
[-0.139, 0.967, -0.215, 0.7],
[0.487, 0.255, 0.835, -1.4], [0.0, 0.0, 0.0, 1.0]])
draw_registration_result(source, target, trans_init)
print("Initial alignment")
evaluation = o3d.pipelines.registration.evaluate_registration(
source, target, threshold, trans_init)
print(evaluation, "\n")
point_to_point_icp(source, target, threshold, trans_init)
point_to_plane_icp(source, target, threshold, trans_init)
|
multiway_registration.py¶
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 | # ----------------------------------------------------------------------------
# - Open3D: www.open3d.org -
# ----------------------------------------------------------------------------
# The MIT License (MIT)
#
# Copyright (c) 2018-2021 www.open3d.org
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in
# all copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
# FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
# IN THE SOFTWARE.
# ----------------------------------------------------------------------------
"""Align multiple pieces of geometry in a global space"""
import open3d as o3d
import numpy as np
def load_point_clouds(voxel_size=0.0):
pcd_data = o3d.data.DemoICPPointClouds()
pcds = []
for i in range(3):
pcd = o3d.io.read_point_cloud(pcd_data.paths[i])
pcd_down = pcd.voxel_down_sample(voxel_size=voxel_size)
pcds.append(pcd_down)
return pcds
def pairwise_registration(source, target, max_correspondence_distance_coarse,
max_correspondence_distance_fine):
print("Apply point-to-plane ICP")
icp_coarse = o3d.pipelines.registration.registration_icp(
source, target, max_correspondence_distance_coarse, np.identity(4),
o3d.pipelines.registration.TransformationEstimationPointToPlane())
icp_fine = o3d.pipelines.registration.registration_icp(
source, target, max_correspondence_distance_fine,
icp_coarse.transformation,
o3d.pipelines.registration.TransformationEstimationPointToPlane())
transformation_icp = icp_fine.transformation
information_icp = o3d.pipelines.registration.get_information_matrix_from_point_clouds(
source, target, max_correspondence_distance_fine,
icp_fine.transformation)
return transformation_icp, information_icp
def full_registration(pcds, max_correspondence_distance_coarse,
max_correspondence_distance_fine):
pose_graph = o3d.pipelines.registration.PoseGraph()
odometry = np.identity(4)
pose_graph.nodes.append(o3d.pipelines.registration.PoseGraphNode(odometry))
n_pcds = len(pcds)
for source_id in range(n_pcds):
for target_id in range(source_id + 1, n_pcds):
transformation_icp, information_icp = pairwise_registration(
pcds[source_id], pcds[target_id],
max_correspondence_distance_coarse,
max_correspondence_distance_fine)
print("Build o3d.pipelines.registration.PoseGraph")
if target_id == source_id + 1: # odometry case
odometry = np.dot(transformation_icp, odometry)
pose_graph.nodes.append(
o3d.pipelines.registration.PoseGraphNode(
np.linalg.inv(odometry)))
pose_graph.edges.append(
o3d.pipelines.registration.PoseGraphEdge(source_id,
target_id,
transformation_icp,
information_icp,
uncertain=False))
else: # loop closure case
pose_graph.edges.append(
o3d.pipelines.registration.PoseGraphEdge(source_id,
target_id,
transformation_icp,
information_icp,
uncertain=True))
return pose_graph
if __name__ == "__main__":
voxel_size = 0.02
pcds_down = load_point_clouds(voxel_size)
o3d.visualization.draw(pcds_down)
print("Full registration ...")
max_correspondence_distance_coarse = voxel_size * 15
max_correspondence_distance_fine = voxel_size * 1.5
with o3d.utility.VerbosityContextManager(
o3d.utility.VerbosityLevel.Debug) as cm:
pose_graph = full_registration(pcds_down,
max_correspondence_distance_coarse,
max_correspondence_distance_fine)
print("Optimizing PoseGraph ...")
option = o3d.pipelines.registration.GlobalOptimizationOption(
max_correspondence_distance=max_correspondence_distance_fine,
edge_prune_threshold=0.25,
reference_node=0)
with o3d.utility.VerbosityContextManager(
o3d.utility.VerbosityLevel.Debug) as cm:
o3d.pipelines.registration.global_optimization(
pose_graph,
o3d.pipelines.registration.GlobalOptimizationLevenbergMarquardt(),
o3d.pipelines.registration.GlobalOptimizationConvergenceCriteria(),
option)
print("Transform points and display")
for point_id in range(len(pcds_down)):
print(pose_graph.nodes[point_id].pose)
pcds_down[point_id].transform(pose_graph.nodes[point_id].pose)
o3d.visualization.draw(pcds_down)
|
pose_graph_optimization.py¶
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 | # ----------------------------------------------------------------------------
# - Open3D: www.open3d.org -
# ----------------------------------------------------------------------------
# The MIT License (MIT)
#
# Copyright (c) 2018-2021 www.open3d.org
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in
# all copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
# FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
# IN THE SOFTWARE.
# ----------------------------------------------------------------------------
import open3d as o3d
import numpy as np
import os
if __name__ == "__main__":
o3d.utility.set_verbosity_level(o3d.utility.VerbosityLevel.Debug)
print("")
print(
"Parameters for o3d.pipelines.registration.PoseGraph optimization ...")
method = o3d.pipelines.registration.GlobalOptimizationLevenbergMarquardt()
criteria = o3d.pipelines.registration.GlobalOptimizationConvergenceCriteria(
)
option = o3d.pipelines.registration.GlobalOptimizationOption()
print("")
print(method)
print(criteria)
print(option)
print("")
print(
"Optimizing Fragment o3d.pipelines.registration.PoseGraph using open3d ..."
)
pose_graph_data = o3d.data.DemoPoseGraphOptimization()
pose_graph_fragment = o3d.io.read_pose_graph(
pose_graph_data.pose_graph_fragment_path)
print(pose_graph_fragment)
o3d.pipelines.registration.global_optimization(pose_graph_fragment, method,
criteria, option)
o3d.io.write_pose_graph(
os.path.join('pose_graph_example_fragment_optimized.json'),
pose_graph_fragment)
print("")
print(
"Optimizing Global o3d.pipelines.registration.PoseGraph using open3d ..."
)
pose_graph_global = o3d.io.read_pose_graph(
pose_graph_data.pose_graph_global_path)
print(pose_graph_global)
o3d.pipelines.registration.global_optimization(pose_graph_global, method,
criteria, option)
o3d.io.write_pose_graph(
os.path.join('pose_graph_example_global_optimized.json'),
pose_graph_global)
|
registration_fgr.py¶
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 | # ----------------------------------------------------------------------------
# - Open3D: www.open3d.org -
# ----------------------------------------------------------------------------
# The MIT License (MIT)
#
# Copyright (c) 2018-2021 www.open3d.org
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in
# all copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
# FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
# IN THE SOFTWARE.
# ----------------------------------------------------------------------------
import open3d as o3d
import argparse
def preprocess_point_cloud(pcd, voxel_size):
pcd_down = pcd.voxel_down_sample(voxel_size)
pcd_down.estimate_normals(
o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size * 2.0,
max_nn=30))
pcd_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
pcd_down,
o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size * 5.0,
max_nn=100))
return (pcd_down, pcd_fpfh)
if __name__ == '__main__':
pcd_data = o3d.data.DemoICPPointClouds()
parser = argparse.ArgumentParser(
'Global point cloud registration example with RANSAC')
parser.add_argument('src',
type=str,
default=pcd_data.paths[0],
nargs='?',
help='path to src point cloud')
parser.add_argument('dst',
type=str,
default=pcd_data.paths[1],
nargs='?',
help='path to dst point cloud')
parser.add_argument('--voxel_size',
type=float,
default=0.05,
help='voxel size in meter used to downsample inputs')
parser.add_argument(
'--distance_multiplier',
type=float,
default=1.5,
help='multipler used to compute distance threshold'
'between correspondences.'
'Threshold is computed by voxel_size * distance_multiplier.')
parser.add_argument('--max_iterations',
type=int,
default=64,
help='number of max FGR iterations')
parser.add_argument(
'--max_tuples',
type=int,
default=1000,
help='max number of accepted tuples for correpsondence filtering')
args = parser.parse_args()
voxel_size = args.voxel_size
distance_threshold = args.distance_multiplier * voxel_size
o3d.utility.set_verbosity_level(o3d.utility.VerbosityLevel.Debug)
print('Reading inputs')
src = o3d.io.read_point_cloud(args.src)
dst = o3d.io.read_point_cloud(args.dst)
print('Downsampling inputs')
src_down, src_fpfh = preprocess_point_cloud(src, voxel_size)
dst_down, dst_fpfh = preprocess_point_cloud(dst, voxel_size)
print('Running FGR')
result = o3d.pipelines.registration.registration_fgr_based_on_feature_matching(
src_down, dst_down, src_fpfh, dst_fpfh,
o3d.pipelines.registration.FastGlobalRegistrationOption(
maximum_correspondence_distance=distance_threshold,
iteration_number=args.max_iterations,
maximum_tuple_count=args.max_tuples))
src.paint_uniform_color([1, 0, 0])
dst.paint_uniform_color([0, 1, 0])
o3d.visualization.draw([src.transform(result.transformation), dst])
|
registration_ransac.py¶
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 | # ----------------------------------------------------------------------------
# - Open3D: www.open3d.org -
# ----------------------------------------------------------------------------
# The MIT License (MIT)
#
# Copyright (c) 2018-2021 www.open3d.org
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in
# all copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
# FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
# IN THE SOFTWARE.
# ----------------------------------------------------------------------------
import open3d as o3d
import argparse
def preprocess_point_cloud(pcd, voxel_size):
pcd_down = pcd.voxel_down_sample(voxel_size)
pcd_down.estimate_normals(
o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size * 2.0,
max_nn=30))
pcd_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
pcd_down,
o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size * 5.0,
max_nn=100))
return (pcd_down, pcd_fpfh)
if __name__ == '__main__':
pcd_data = o3d.data.DemoICPPointClouds()
parser = argparse.ArgumentParser(
'Global point cloud registration example with RANSAC')
parser.add_argument('src',
type=str,
default=pcd_data.paths[0],
nargs='?',
help='path to src point cloud')
parser.add_argument('dst',
type=str,
default=pcd_data.paths[1],
nargs='?',
help='path to dst point cloud')
parser.add_argument('--voxel_size',
type=float,
default=0.05,
help='voxel size in meter used to downsample inputs')
parser.add_argument(
'--distance_multiplier',
type=float,
default=1.5,
help='multipler used to compute distance threshold'
'between correspondences.'
'Threshold is computed by voxel_size * distance_multiplier.')
parser.add_argument('--max_iterations',
type=int,
default=1000000,
help='number of max RANSAC iterations')
parser.add_argument('--confidence',
type=float,
default=0.999,
help='RANSAC confidence')
parser.add_argument(
'--mutual_filter',
action='store_true',
help='whether to use mutual filter for putative correspondences')
args = parser.parse_args()
voxel_size = args.voxel_size
distance_threshold = args.distance_multiplier * voxel_size
o3d.utility.set_verbosity_level(o3d.utility.VerbosityLevel.Debug)
print('Reading inputs')
src = o3d.io.read_point_cloud(args.src)
dst = o3d.io.read_point_cloud(args.dst)
print('Downsampling inputs')
src_down, src_fpfh = preprocess_point_cloud(src, voxel_size)
dst_down, dst_fpfh = preprocess_point_cloud(dst, voxel_size)
print('Running RANSAC')
result = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
src_down,
dst_down,
src_fpfh,
dst_fpfh,
mutual_filter=args.mutual_filter,
max_correspondence_distance=distance_threshold,
estimation_method=o3d.pipelines.registration.
TransformationEstimationPointToPoint(False),
ransac_n=3,
checkers=[
o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength(
0.9),
o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(
distance_threshold)
],
criteria=o3d.pipelines.registration.RANSACConvergenceCriteria(
args.max_iterations, args.confidence))
src.paint_uniform_color([1, 0, 0])
dst.paint_uniform_color([0, 1, 0])
o3d.visualization.draw([src.transform(result.transformation), dst])
|
rgbd_integration_uniform.py¶
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 | # ----------------------------------------------------------------------------
# - Open3D: www.open3d.org -
# ----------------------------------------------------------------------------
# The MIT License (MIT)
#
# Copyright (c) 2018-2021 www.open3d.org
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in
# all copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
# FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
# IN THE SOFTWARE.
# ----------------------------------------------------------------------------
import open3d as o3d
import numpy as np
import os, sys
pyexample_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
sys.path.append(pyexample_path)
from open3d_example import read_trajectory
if __name__ == "__main__":
rgbd_data = o3d.data.SampleRedwoodRGBDImages()
camera_poses = read_trajectory(rgbd_data.odometry_log_path)
camera_intrinsics = o3d.camera.PinholeCameraIntrinsic(
o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault)
volume = o3d.pipelines.integration.UniformTSDFVolume(
length=4.0,
resolution=512,
sdf_trunc=0.04,
color_type=o3d.pipelines.integration.TSDFVolumeColorType.RGB8,
)
for i in range(len(camera_poses)):
print("Integrate {:d}-th image into the volume.".format(i))
color = o3d.io.read_image(rgbd_data.color_paths[i])
depth = o3d.io.read_image(rgbd_data.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,
camera_intrinsics,
np.linalg.inv(camera_poses[i].pose),
)
print("Extract triangle mesh")
mesh = volume.extract_triangle_mesh()
mesh.compute_vertex_normals()
o3d.visualization.draw_geometries([mesh])
print("Extract voxel-aligned debugging point cloud")
voxel_pcd = volume.extract_voxel_point_cloud()
o3d.visualization.draw_geometries([voxel_pcd])
print("Extract voxel-aligned debugging voxel grid")
voxel_grid = volume.extract_voxel_grid()
# o3d.visualization.draw_geometries([voxel_grid])
# print("Extract point cloud")
# pcd = volume.extract_point_cloud()
# o3d.visualization.draw_geometries([pcd])
|
rgbd_odometry.py¶
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 | # ----------------------------------------------------------------------------
# - Open3D: www.open3d.org -
# ----------------------------------------------------------------------------
# The MIT License (MIT)
#
# Copyright (c) 2018-2021 www.open3d.org
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in
# all copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
# FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
# IN THE SOFTWARE.
# ----------------------------------------------------------------------------
"""Find camera movement between two consecutive RGBD image pairs"""
import open3d as o3d
import numpy as np
if __name__ == "__main__":
pinhole_camera_intrinsic = o3d.camera.PinholeCameraIntrinsic(
o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault)
rgbd_data = o3d.data.SampleRedwoodRGBDImages()
source_color = o3d.io.read_image(rgbd_data.color_paths[0])
source_depth = o3d.io.read_image(rgbd_data.depth_paths[0])
target_color = o3d.io.read_image(rgbd_data.color_paths[1])
target_depth = o3d.io.read_image(rgbd_data.depth_paths[1])
source_rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
source_color, source_depth)
target_rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
target_color, target_depth)
target_pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
target_rgbd_image, pinhole_camera_intrinsic)
option = o3d.pipelines.odometry.OdometryOption()
odo_init = np.identity(4)
print(option)
[success_color_term, trans_color_term,
info] = o3d.pipelines.odometry.compute_rgbd_odometry(
source_rgbd_image, target_rgbd_image,
pinhole_camera_intrinsic, odo_init,
o3d.pipelines.odometry.RGBDOdometryJacobianFromColorTerm(), option)
[success_hybrid_term, trans_hybrid_term,
info] = o3d.pipelines.odometry.compute_rgbd_odometry(
source_rgbd_image, target_rgbd_image,
pinhole_camera_intrinsic, odo_init,
o3d.pipelines.odometry.RGBDOdometryJacobianFromHybridTerm(), option)
if success_color_term:
print("Using RGB-D Odometry")
print(trans_color_term)
source_pcd_color_term = o3d.geometry.PointCloud.create_from_rgbd_image(
source_rgbd_image, pinhole_camera_intrinsic)
source_pcd_color_term.transform(trans_color_term)
o3d.visualization.draw([target_pcd, source_pcd_color_term])
if success_hybrid_term:
print("Using Hybrid RGB-D Odometry")
print(trans_hybrid_term)
source_pcd_hybrid_term = o3d.geometry.PointCloud.create_from_rgbd_image(
source_rgbd_image, pinhole_camera_intrinsic)
source_pcd_hybrid_term.transform(trans_hybrid_term)
o3d.visualization.draw([target_pcd, source_pcd_hybrid_term])
|
robust_icp.py¶
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 | # ----------------------------------------------------------------------------
# - Open3D: www.open3d.org -
# ----------------------------------------------------------------------------
# The MIT License (MIT)
#
# Copyright (c) 2018-2021 www.open3d.org
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in
# all copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
# FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
# IN THE SOFTWARE.
# ----------------------------------------------------------------------------
"""Outlier rejection using robust kernels for ICP"""
import open3d as o3d
import numpy as np
import copy
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([source_temp, target_temp])
def apply_noise(pcd, mu, sigma):
noisy_pcd = copy.deepcopy(pcd)
points = np.asarray(noisy_pcd.points)
points += np.random.normal(mu, sigma, size=points.shape)
noisy_pcd.points = o3d.utility.Vector3dVector(points)
return noisy_pcd
if __name__ == "__main__":
pcd_data = o3d.data.DemoICPPointClouds()
source = o3d.io.read_point_cloud(pcd_data.paths[0])
target = o3d.io.read_point_cloud(pcd_data.paths[1])
trans_init = np.asarray([[0.862, 0.011, -0.507, 0.5],
[-0.139, 0.967, -0.215, 0.7],
[0.487, 0.255, 0.835, -1.4], [0.0, 0.0, 0.0, 1.0]])
# Mean and standard deviation.
mu, sigma = 0, 0.1
source_noisy = apply_noise(source, mu, sigma)
print("Displaying source point cloud + noise:")
o3d.visualization.draw([source_noisy])
print(
"Displaying original source and target point cloud with initial transformation:"
)
draw_registration_result(source, target, trans_init)
threshold = 1.0
print("Using the noisy source pointcloud to perform robust ICP.\n")
print("Robust point-to-plane ICP, threshold={}:".format(threshold))
loss = o3d.pipelines.registration.TukeyLoss(k=sigma)
print("Using robust loss:", loss)
p2l = o3d.pipelines.registration.TransformationEstimationPointToPlane(loss)
reg_p2l = o3d.pipelines.registration.registration_icp(
source_noisy, target, threshold, trans_init, p2l)
print(reg_p2l)
print("Transformation is:")
print(reg_p2l.transformation)
draw_registration_result(source, target, reg_p2l.transformation)
|