def full_registration(path,max_correspondence_distance_coarse,
max_correspondence_distance_fine):
global N_Neighbours, LABEL_INTERVAL, n_pcds
pose_graph = PoseGraph()
odometry = np.identity(4)
pose_graph.nodes.append(PoseGraphNode(odometry))
pcds = [[] for i in range(n_pcds)]
for source_id in trange(n_pcds):
if source_id > 0:
pcds[source_id-1] = []
# for target_id in range(source_id + 1, min(source_id + N_Neighbours,n_pcds)):
for target_id in range(source_id + 1, n_pcds, max(1,int(n_pcds/N_Neighbours))):
# print(source_id, target_id)
# derive pairwise registration through feature matching
color_src, depth_src = load_images(path, source_id)
color_dst, depth_dst = load_images(path, target_id)
res = marker_registration((color_src, depth_src),
(color_dst, depth_dst))
if res is None and target_id != source_id + 1:
# ignore such connections
continue
if not pcds[source_id]:
pcds[source_id] = load_pcd(path, source_id, downsample = True)
# print(type(pcds[source_id]))
# open3d.io.write_point_cloud(sys.argv[1]+"PCD/%d.pcd"%source_id, pcds[source_id])
if not pcds[target_id]:
pcds[target_id] = load_pcd(path, target_id, downsample = True)
# open3d.io.write_point_cloud(sys.argv[1]+"PCD/%d.pcd"%target_id, pcds[target_id])
if res is None:
# if marker_registration fails, perform pointcloud matching
transformation_icp, information_icp = icp(
pcds[source_id], pcds[target_id], voxel_size, max_correspondence_distance_coarse,
max_correspondence_distance_fine, method = ICP_METHOD)
else:
transformation_icp = res
information_icp = get_information_matrix_from_point_clouds(
pcds[source_id], pcds[target_id], max_correspondence_distance_fine,
transformation_icp)
# print('----')
# print(np.linalg.inv(transformation_icp))
# print(information_icp)
# print('----')
if target_id == source_id + 1:
# odometry
odometry = np.dot(transformation_icp, odometry)
pose_graph.nodes.append(PoseGraphNode(np.linalg.inv(odometry)))
pose_graph.edges.append(PoseGraphEdge(source_id, target_id,
transformation_icp, information_icp, uncertain = False))
else:
# loop closure
pose_graph.edges.append(PoseGraphEdge(source_id, target_id,
transformation_icp, information_icp, uncertain = True))
return pose_graph
def full_registration(pcds_down,cads,depths, max_correspondence_distance_coarse,max_correspondence_distance_fine):
"""
perform pairwise registration and build pose graph for up to N_Neighbours
Parameters
----------
pcds_down : List of open3d.Pointcloud instances
Downampled 6D pontcloud of the unalligned segments
max_correspondence_distance_coarse : float
The max correspondence distance used for the course ICP during the process
of coarse to fine registration
max_correspondence_distance_fine : float
The max correspondence distance used for the fine ICP during the process
of coarse to fine registration
Returns
----------
pose_graph: an open3d.PoseGraph instance
Stores poses of each segment in the node and pairwise correlation in vertice
"""
global N_Neighbours
pose_graph = o3d.pipelines.registration.PoseGraph()
odometry = np.identity(4)
pose_graph.nodes.append(o3d.pipelines.registration.PoseGraphNode(odometry))
n_pcds = len(pcds_down)
for source_id in trange(n_pcds):
for target_id in range(source_id + 1, min(source_id + N_Neighbours,n_pcds)):
# derive pairwise registration through feature matching
color_src = cads[source_id]
depth_src = depths[source_id]
color_dst = cads[target_id]
depth_dst = depths[target_id]
res = feature_registration((color_src, depth_src),
(color_dst, depth_dst))
if res is None:
# if feature matching fails, perform pointcloud matching
transformation_icp, information_icp = icp(
pcds_down[source_id], pcds_down[target_id],max_correspondence_distance_coarse,
max_correspondence_distance_fine, method = RECON_METHOD)
else:
transformation_icp = res
information_icp = o3d.pipelines.registration.get_information_matrix_from_point_clouds(
pcds_down[source_id], pcds_down[target_id], max_correspondence_distance_fine,
transformation_icp)
information_icp *= 1.2 ** (target_id - source_id - 1)
if target_id == source_id + 1:
# odometry
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
pose_graph.edges.append(o3d.pipelines.registration.PoseGraphEdge(source_id, target_id,
transformation_icp, information_icp, uncertain=True))
return pose_graph