def register2Fragments(id1, id2, keyptspath, descpath, resultpath, desc_name='ppf'): cloud_bin_s = f'cloud_bin_{id1}' cloud_bin_t = f'cloud_bin_{id2}' write_file = f'{cloud_bin_s}_{cloud_bin_t}.rt.txt' if os.path.exists(os.path.join(resultpath, write_file)): # print(f"{write_file} already exists.") return 0, 0, 0 source_keypts = get_keypts(keyptspath, cloud_bin_s) target_keypts = get_keypts(keyptspath, cloud_bin_t) # print(source_keypts.shape) source_desc = get_desc(descpath, cloud_bin_s, desc_name=desc_name) target_desc = get_desc(descpath, cloud_bin_t, desc_name=desc_name) source_desc = np.nan_to_num(source_desc) target_desc = np.nan_to_num(target_desc) key = f'{cloud_bin_s.split("_")[-1]}_{cloud_bin_t.split("_")[-1]}' if key not in gtLog.keys(): num_inliers = 0 inlier_ratio = 0 gt_flag = 0 else: # find mutually cloest point. corr = calculate_M(source_desc, target_desc) gtTrans = gtLog[key] frag1 = source_keypts[corr[:, 0]] frag2_pc = open3d.PointCloud() frag2_pc.points = open3d.utility.Vector3dVector(target_keypts[corr[:, 1]]) frag2_pc.transform(gtTrans) frag2 = np.asarray(frag2_pc.points) distance = np.sqrt(np.sum(np.power(frag1 - frag2, 2), axis=1)) num_inliers = np.sum(distance < 0.1) inlier_ratio = num_inliers / len(distance) gt_flag = 1 s = f"{cloud_bin_s}\t{cloud_bin_t}\t{num_inliers}\t{inlier_ratio:.8f}\t{gt_flag}" with open(os.path.join(resultpath, f'{cloud_bin_s}_{cloud_bin_t}.rt.txt'), 'w+') as f: f.write(s) return num_inliers, inlier_ratio, gt_flag
def register2Fragments(id1, id2, pcdpath, keyptspath, descpath, desc_name='ppf'): cloud_bin_s = f'cloud_bin_{id1}' cloud_bin_t = f'cloud_bin_{id2}' # 1. load point cloud, keypoints, descriptors original_source_pc = get_pcd(pcdpath, cloud_bin_s) original_target_pc = get_pcd(pcdpath, cloud_bin_t) print("original source:", original_source_pc) print("original target:", original_target_pc) # downsample and estimate the normals voxel_size = 0.02 source_pc = open3d.geometry.voxel_down_sample(original_source_pc, voxel_size) open3d.geometry.estimate_normals(source_pc, open3d.KDTreeSearchParamHybrid(radius=voxel_size * 2, max_nn=30)) target_pc = open3d.geometry.voxel_down_sample(original_target_pc, voxel_size) open3d.geometry.estimate_normals(target_pc, open3d.KDTreeSearchParamHybrid(radius=voxel_size * 2, max_nn=30)) print("downsampled source:", source_pc) print("downsampled target:", target_pc) # load keypoints and descriptors source_keypts = get_keypts(keyptspath, cloud_bin_s) target_keypts = get_keypts(keyptspath, cloud_bin_t) # print(source_keypts.shape) source_desc = get_desc(descpath, cloud_bin_s, desc_name=desc_name) target_desc = get_desc(descpath, cloud_bin_t, desc_name=desc_name) # 2. ransac # ransac_result = ransac_based_on_correspondence(source_keypts, target_keypts, source_desc, target_desc) ransac_result = ransac_based_on_feature_matching(source_keypts, target_keypts, source_desc, target_desc) print("RANSAC Correspondence_set:", len(ransac_result.correspondence_set)) print(ransac_result.transformation) # 3. refine with ICP icp_result = icp_refine(source_pc, target_pc, ransac_result.transformation, voxel_size * 0.4) print("ICP Correspondence_set:", len(ransac_result.correspondence_set)) print(icp_result.transformation) # 4. transform the target_pc, so that source and target are in the same coordinates. target_pc.transform(icp_result.transformation) # 5. compute the alignment start_time = time.time() align = cal_alignment(original_source_pc, original_target_pc, 0.05) print(time.time() - start_time) print("align:", align) print("trans:", icp_result.transformation)
def register2Fragments(id1, id2, keyptspath, descpath, resultpath, logpath, gtLog, desc_name, inlier_ratio, distance_threshold): """ Register point cloud {id1} and {id2} using the keypts location and descriptors. """ cloud_bin_s = f'cloud_bin_{id1}' cloud_bin_t = f'cloud_bin_{id2}' write_file = f'{cloud_bin_s}_{cloud_bin_t}.rt.txt' if os.path.exists(os.path.join(resultpath, write_file)): return 0, 0, 0 source_keypts = get_keypts(keyptspath, cloud_bin_s) target_keypts = get_keypts(keyptspath, cloud_bin_t) source_desc = get_desc(descpath, cloud_bin_s, desc_name) target_desc = get_desc(descpath, cloud_bin_t, desc_name) source_desc = np.nan_to_num(source_desc) target_desc = np.nan_to_num(target_desc) # Select {num_keypts} points based on the scores. The descriptors and keypts are already sorted based on the detection score. num_keypts = 250 source_keypts = source_keypts[-num_keypts:, :] source_desc = source_desc[-num_keypts:, :] target_keypts = target_keypts[-num_keypts:, :] target_desc = target_desc[-num_keypts:, :] # Select {num_keypts} points randomly. # num_keypts = 250 # source_indices = np.random.choice(range(source_keypts.shape[0]), num_keypts) # target_indices = np.random.choice(range(target_keypts.shape[0]), num_keypts) # source_keypts = source_keypts[source_indices, :] # source_desc = source_desc[source_indices, :] # target_keypts = target_keypts[target_indices, :] # target_desc = target_desc[target_indices, :] key = f'{cloud_bin_s.split("_")[-1]}_{cloud_bin_t.split("_")[-1]}' if key not in gtLog.keys(): # skip the pairs that have less than 30% overlap. num_inliers = 0 inlier_ratio = 0 gt_flag = 0 else: # build correspondence set in feature space. corr = build_correspondence(source_desc, target_desc) # calculate the inlier ratio, this is for Feature Matching Recall. gt_trans = gtLog[key] frag1 = source_keypts[corr[:, 0]] frag2_pc = open3d.PointCloud() frag2_pc.points = open3d.utility.Vector3dVector(target_keypts[corr[:, 1]]) frag2_pc.transform(gt_trans) frag2 = np.asarray(frag2_pc.points) distance = np.sqrt(np.sum(np.power(frag1 - frag2, 2), axis=1)) num_inliers = np.sum(distance < distance_threshold) if num_inliers / len(distance) < inlier_ratio: print(key) print("num_corr:", len(corr), "inlier_ratio:", num_inliers / len(distance)) inlier_ratio = num_inliers / len(distance) gt_flag = 1 # calculate the transformation matrix using RANSAC, this is for Registration Recall. source_pcd = open3d.PointCloud() source_pcd.points = open3d.utility.Vector3dVector(source_keypts) target_pcd = open3d.PointCloud() target_pcd.points = open3d.utility.Vector3dVector(target_keypts) s_desc = open3d.registration.Feature() s_desc.data = source_desc.T t_desc = open3d.registration.Feature() t_desc.data = target_desc.T result = open3d.registration_ransac_based_on_feature_matching( source_pcd, target_pcd, s_desc, t_desc, 0.05, open3d.TransformationEstimationPointToPoint(False), 3, [open3d.CorrespondenceCheckerBasedOnEdgeLength(0.9), open3d.CorrespondenceCheckerBasedOnDistance(0.05)], open3d.RANSACConvergenceCriteria(50000, 1000)) # write the transformation matrix into .log file for evaluation. with open(os.path.join(logpath, f'{desc_name}_{timestr}.log'), 'a+') as f: trans = result.transformation trans = np.linalg.inv(trans) s1 = f'{id1}\t {id2}\t 37\n' f.write(s1) f.write(f"{trans[0,0]}\t {trans[0,1]}\t {trans[0,2]}\t {trans[0,3]}\t \n") f.write(f"{trans[1,0]}\t {trans[1,1]}\t {trans[1,2]}\t {trans[1,3]}\t \n") f.write(f"{trans[2,0]}\t {trans[2,1]}\t {trans[2,2]}\t {trans[2,3]}\t \n") f.write(f"{trans[3,0]}\t {trans[3,1]}\t {trans[3,2]}\t {trans[3,3]}\t \n") # write the result into resultpath so that it can be re-shown. s = f"{cloud_bin_s}\t{cloud_bin_t}\t{num_inliers}\t{inlier_ratio:.8f}\t{gt_flag}" with open(os.path.join(resultpath, f'{cloud_bin_s}_{cloud_bin_t}.rt.txt'), 'w+') as f: f.write(s) return num_inliers, inlier_ratio, gt_flag
def register2Fragments(id1, id2, keyptspath, descpath, resultpath, gtLog, desc_name, inlier_ratio, distance_threshold): """ Register point cloud {id1} and {id2} using the keypts location and descriptors. """ # cloud_bin_s = f'Hokuyo_{id1}' # cloud_bin_t = f'Hokuyo_{id2}' cloud_bin_s = f'cloud_bin_{id1}' cloud_bin_t = f'cloud_bin_{id2}' write_file = f'{cloud_bin_s}_{cloud_bin_t}.rt.txt' if os.path.exists(os.path.join(resultpath, write_file)): return 0, 0, 0 source_keypts = get_keypts(keyptspath, cloud_bin_s) target_keypts = get_keypts(keyptspath, cloud_bin_t) source_desc = get_desc(descpath, cloud_bin_s, desc_name) target_desc = get_desc(descpath, cloud_bin_t, desc_name) source_desc = np.nan_to_num(source_desc) target_desc = np.nan_to_num(target_desc) # Select {num_keypts} points based on the scores. The descriptors and keypts are already sorted based on the detection score. num_keypts = 250 source_keypts = source_keypts[-num_keypts:, :] source_desc = source_desc[-num_keypts:, :] target_keypts = target_keypts[-num_keypts:, :] target_desc = target_desc[-num_keypts:, :] # Select {num_keypts} points randomly. # num_keypts = 250 # source_indices = np.random.choice(range(source_keypts.shape[0]), num_keypts) # target_indices = np.random.choice(range(target_keypts.shape[0]), num_keypts) # source_keypts = source_keypts[source_indices, :] # source_desc = source_desc[source_indices, :] # target_keypts = target_keypts[target_indices, :] # target_desc = target_desc[target_indices, :] key = f'{cloud_bin_s.split("_")[-1]}_{cloud_bin_t.split("_")[-1]}' if key not in gtLog.keys(): # skip the pairs that have less than 30% overlap. num_inliers = 0 inlier_ratio = 0 gt_flag = 0 else: # find mutually cloest point. corr = build_correspondence(source_desc, target_desc) gtTrans = gtLog[key] frag1 = source_keypts[corr[:, 0]] frag2_pc = open3d.PointCloud() frag2_pc.points = open3d.utility.Vector3dVector(target_keypts[corr[:, 1]]) frag2_pc.transform(gtTrans) frag2 = np.asarray(frag2_pc.points) distance = np.sqrt(np.sum(np.power(frag1 - frag2, 2), axis=1)) num_inliers = np.sum(distance < distance_threshold) if num_inliers / len(distance) < inlier_ratio: print(key) print("num_corr:", len(corr), "inlier_ratio:", num_inliers / len(distance)) inlier_ratio = num_inliers / len(distance) gt_flag = 1 # write the result into resultpath so that it can be re-shown. s = f"{cloud_bin_s}\t{cloud_bin_t}\t{num_inliers}\t{inlier_ratio:.8f}\t{gt_flag}" with open(os.path.join(resultpath, f'{cloud_bin_s}_{cloud_bin_t}.rt.txt'), 'w+') as f: f.write(s) return num_inliers, inlier_ratio, gt_flag