def multiscale_icp(source,
target,
voxel_size,
max_iter,
init_transformation=np.identity(4)):
current_transformation = init_transformation
for i, scale in enumerate(range(len(max_iter))): # multi-scale approach
iter = max_iter[scale]
distance_threshold = 0.07
print("voxel_size {}".format(voxel_size[scale]))
source_down = source.voxel_down_sample(voxel_size[scale])
target_down = target.voxel_down_sample(voxel_size[scale])
source_down.estimate_normals(
o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size[scale] *
2.0,
max_nn=30))
target_down.estimate_normals(
o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size[scale] *
2.0,
max_nn=30))
result_icp = o3d.pipelines.registration.registration_colored_icp(
source_down, target_down, distance_threshold,
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
draw_registration_result_original_color(source, target,
current_transformation)
print(current_transformation)
return result_icp.transformation
def multiscale_icp(source,
target,
voxel_size,
max_iter,
config,
init_transformation=np.identity(4)):
current_transformation = init_transformation
for i, scale in enumerate(range(len(max_iter))): # multi-scale approach
iter = max_iter[scale]
distance_threshold = config["voxel_size"] * 1.4
print("voxel_size {}".format(voxel_size[scale]))
source_down = source.voxel_down_sample(voxel_size[scale])
target_down = target.voxel_down_sample(voxel_size[scale])
if config["icp_method"] == "point_to_point":
result_icp = o3d.pipelines.registration.registration_icp(
source_down, target_down, distance_threshold,
current_transformation,
o3d.pipelines.registration.
TransformationEstimationPointToPoint(),
o3d.pipelines.registration.ICPConvergenceCriteria(
max_iteration=iter))
else:
source_down.estimate_normals(
o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size[scale] *
2.0,
max_nn=30))
target_down.estimate_normals(
o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size[scale] *
2.0,
max_nn=30))
if config["icp_method"] == "point_to_plane":
result_icp = o3d.pipelines.registration.registration_icp(
source_down, target_down, distance_threshold,
current_transformation,
o3d.pipelines.registration.
TransformationEstimationPointToPlane(),
o3d.pipelines.registration.ICPConvergenceCriteria(
max_iteration=iter))
if config["icp_method"] == "color":
result_icp = o3d.pipelines.registration.registration_colored_icp(
source_down, target_down, voxel_size[scale],
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
if i == len(max_iter) - 1:
information_matrix = o3d.pipelines.registration.get_information_matrix_from_point_clouds(
source_down, target_down, voxel_size[scale] * 1.4,
result_icp.transformation)
if config["debug_mode"]:
draw_registration_result_original_color(source, target,
result_icp.transformation)
return (result_icp.transformation, information_matrix)
def local_refinement(source, target, transformation_init, config):
voxel_size = config["voxel_size"]
(transformation, information) = \
multiscale_icp(
source, target,
[voxel_size, voxel_size/2.0, voxel_size/4.0], [50, 30, 14],
config, transformation_init)
if config["debug_mode"]:
draw_registration_result_original_color(source, target, transformation)
return (transformation, information)
def register_point_cloud_pair(ply_file_names, s, t, transformation_init,
config):
print("reading %s ..." % ply_file_names[s])
source = o3d.io.read_point_cloud(ply_file_names[s])
print("reading %s ..." % ply_file_names[t])
target = o3d.io.read_point_cloud(ply_file_names[t])
if config["debug_mode"]:
draw_registration_result_original_color(source, target,
transformation_init)
(transformation, information) = \
local_refinement(source, target, transformation_init, config)
if config["debug_mode"]:
draw_registration_result_original_color(source, target, transformation)
print(transformation)
print(information)
return (transformation, information)
