def ransac_based_on_correspondence(source_keypts, target_keypts, source_desc, target_desc): source_pc = open3d.PointCloud() source_pc.points = open3d.utility.Vector3dVector(source_keypts) target_pc = open3d.PointCloud() target_pc.points = open3d.utility.Vector3dVector(target_keypts) voxel_size = 0.05 # source_pc = open3d.geometry.voxel_down_sample(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(target_pc, voxel_size) open3d.geometry.estimate_normals(target_pc, open3d.KDTreeSearchParamHybrid(radius=voxel_size * 2, max_nn=30)) start_time = time.time() corr = calculate_M(source_desc[0:2000], target_desc[0:2000]) print(time.time() - start_time) print("Num of correspondence", len(corr)) result = open3d.registration_ransac_based_on_correspondence( source=source_pc, target=target_pc, corres=open3d.utility.Vector2iVector(corr), max_correspondence_distance=0.05, estimation_method=open3d.registration.TransformationEstimationPointToPoint(False), ransac_n=4, criteria=open3d.registration.RANSACConvergenceCriteria(4000000, 500) ) return result
def estimateTransform(self, source, target): source = od.read_point_cloud(source) target = od.read_point_cloud(target) current_transformation = self.base_transform for scale in range(len(self.voxel_radius)): iterations = self.max_iter[scale] radius = self.voxel_radius[scale] source_down = od.voxel_down_sample(source, radius) target_down = od.voxel_down_sample(target, radius) od.estimate_normals( source_down, od.KDTreeSearchParamHybrid(radius=2 * radius, max_nn=self.max_nn)) od.estimate_normals( target_down, od.KDTreeSearchParamHybrid(radius=2 * radius, max_nn=self.max_nn)) result_icp = od.registration_colored_icp( source_down, target_down, radius, current_transformation, od.ICPConvergenceCriteria( relative_fitness=self.relative_fitness, relative_rmse=self.relative_rmse, max_iteration=iterations)) current_transformation = result_icp.transformation # self.draw_registration_result_original_color( # source_down, target_down, current_transformation) return current_transformation
def sample_point_cloud_feature(point_cloud: op3.PointCloud, voxel_size: float, verbose=False): """ Down sample and sample the point cloud feature :param point_cloud: an object of Open3D :param voxel_size: a float value, that is how sparse you want the sample points is :param verbose: a boolean value, display notification and visualization when True and no notification when False :return: 2 objects of Open3D, that are down-sampled point cloud and point cloud feature fpfh """ if verbose: print(":: Downsample with a voxel size %.3f." % voxel_size) point_cloud_down_sample = op3.voxel_down_sample(point_cloud, voxel_size) radius_normal = voxel_size * 2 if verbose: print(":: Estimate normal with search radius %.3f." % radius_normal) op3.estimate_normals( point_cloud_down_sample, op3.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30)) radius_feature = voxel_size * 5 if verbose: print(":: Compute FPFH feature with search radius %.3f." % radius_feature) point_cloud_fpfh = op3.compute_fpfh_feature( point_cloud_down_sample, op3.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100)) return point_cloud_down_sample, point_cloud_fpfh
def ransac_based_on_feature_matching(source_keypts, target_keypts, source_desc, target_desc): source_pc = open3d.PointCloud() source_pc.points = open3d.utility.Vector3dVector(source_keypts) target_pc = open3d.PointCloud() target_pc.points = open3d.utility.Vector3dVector(target_keypts) source_feature = open3d.registration.Feature() source_feature.data = source_desc.transpose() target_feature = open3d.registration.Feature() target_feature.data = target_desc.transpose() voxel_size = 0.05 # source_pc = open3d.geometry.voxel_down_sample(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(target_pc, voxel_size) open3d.geometry.estimate_normals(target_pc, open3d.KDTreeSearchParamHybrid(radius=voxel_size * 2, max_nn=30)) # source_feature = open3d.registration.compute_fpfh_feature(source_pc, open3d.KDTreeSearchParamHybrid(radius=voxel_size * 5, max_nn=100)) # target_feature = open3d.registration.compute_fpfh_feature(target_pc, open3d.KDTreeSearchParamHybrid(radius=voxel_size * 5, max_nn=100)) result = open3d.registration_ransac_based_on_feature_matching( source=source_pc, target=target_pc, source_feature=source_feature, target_feature=target_feature, max_correspondence_distance=0.05, estimation_method=open3d.registration.TransformationEstimationPointToPoint(False), ransac_n=3, checkers=[], criteria=open3d.registration.RANSACConvergenceCriteria(4000000, 500) ) return result
def _get_features(cloud): o3.estimate_normals( cloud, o3.KDTreeSearchParamHybrid(radius=Param["normal_radius"], max_nn=Param["normal_max_nn"])) return o3.compute_fpfh_feature( cloud, o3.KDTreeSearchParamHybrid(radius=Param["feature_radius"], max_nn=Param["feature_max_nn"]))
def feature_(pcd): o3d.estimate_normals( pcd, o3d.KDTreeSearchParamHybrid(radius=param['radius_normal'], max_nn=param['maxnn_normal'])) ft = o3d.compute_fpfh_feature( pcd, o3d.KDTreeSearchParamHybrid(radius=param['radius_feature'], max_nn=param['maxnn_feature'])) return ft
def registration(voxel_size, threshold, source_points, target_points, trans_init): source, target, source_down, target_down, source_fpfh, target_fpfh = \ prepare_dataset(voxel_size, source_points, target_points, trans_init) result_ransac = execute_global_registration(source_down, target_down, source_fpfh, target_fpfh, voxel_size) # perform ICP registration o3d.estimate_normals(source, search_param=o3d.KDTreeSearchParamHybrid(radius=1, max_nn=30)) o3d.estimate_normals(target, search_param=o3d.KDTreeSearchParamHybrid(radius=1, max_nn=30)) trans_init = result_ransac.transformation # print("Initial alignment") evaluation = o3d.registration.evaluate_registration( source, target, threshold, trans_init) # print(evaluation) # print("Apply point-to-point ICP") reg_p2p = o3d.registration.registration_icp( source, target, threshold, trans_init, o3d.registration.TransformationEstimationPointToPoint()) # print(reg_p2p) # print("Transformation is:") # print(reg_p2p.transformation) # draw_registration_result(source, target, reg_p2p.transformation) # second ICP trans_init2 = reg_p2p.transformation # print("Apply 2nd point-to-point ICP") reg_p2p2 = o3d.registration.registration_icp( source, target, threshold, trans_init2, o3d.registration.TransformationEstimationPointToPoint()) # draw_registration_result(source, target, reg_p2p2.transformation) # third ICP trans_init3 = reg_p2p2.transformation # print("Apply 3rd point-to-point ICP") reg_p2p3 = o3d.registration.registration_icp( source, target, threshold, trans_init3, o3d.registration.TransformationEstimationPointToPoint()) # print('checking rms') # print(reg_p2p3.inlier_rmse) # print(reg_p2p3) # print("Transformation 3 is:") # print(reg_p2p3.transformation) # print("") # draw_registration_result(source, target, reg_p2p3.transformation) return reg_p2p3
def preprocess_point_cloud(pcd, voxel_size): pn.estimate_normals(pcd) pcd_down = pn.voxel_down_sample(pcd, voxel_size) radius_normal = voxel_size * 2 pn.estimate_normals( pcd_down, pn.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=10)) radius_feature = voxel_size * 5 pcd_fpfh = pn.compute_fpfh_feature( pcd_down, pn.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=50)) return pcd_down, pcd_fpfh
def preprocess_point_cloud(pcd, voxel_size): print(":: Downsample with a voxel size %.3f." % voxel_size) pcd_down = op3.voxel_down_sample(pcd, voxel_size) radius_normal = voxel_size * 2 print(":: Estimate normal with search radius %.3f." % radius_normal) op3.estimate_normals( pcd_down, op3.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30)) radius_feature = voxel_size * 5 print(":: Compute FPFH feature with search radius %.3f." % radius_feature) pcd_fpfh = op3.compute_fpfh_feature( pcd_down, op3.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100)) return pcd_down, pcd_fpfh
def ViewPLY(path): # 読み込み pointcloud = open3d.read_point_cloud(path) # ダウンサンプリング # 法線推定できなくなるので不採用 pointcloud = open3d.voxel_down_sample(pointcloud, 10) # 法線推定 open3d.estimate_normals(pointcloud, search_param=open3d.KDTreeSearchParamHybrid( radius=20, max_nn=30)) # 法線の方向を視点ベースでそろえる open3d.orient_normals_towards_camera_location(pointcloud, camera_location=np.array( [0., 10., 10.], dtype="float64")) # 可視化 open3d.draw_geometries([pointcloud]) # numpyに変換 points = np.asarray(pointcloud.points) normals = np.asarray(pointcloud.normals) print("points:{}".format(points.shape[0])) X, Y, Z = Disassemble(points) # OBB生成 _, _, length = buildOBB(points) print("OBB_length: {}".format(length)) return points, X, Y, Z, normals, length
def o3d_estimate_normals(source): # downsample and estimate normals source = o3d.voxel_down_sample(source, voxel_size = 2.5) o3d.estimate_normals(source, search_param = o3d.KDTreeSearchParamHybrid( radius = 5, max_nn = 30)) return source
def NormalEstimate(points): """ 法線推定 """ #点群をnp配列⇒open3d形式に pointcloud = open3d.PointCloud() pointcloud.points = open3d.Vector3dVector(points) # 法線推定 open3d.estimate_normals(pointcloud, search_param=open3d.KDTreeSearchParamHybrid( radius=5, max_nn=30)) # 法線の方向を視点ベースでそろえる open3d.orient_normals_towards_camera_location(pointcloud, camera_location=np.array( [0., 10., 10.], dtype="float64")) #nキーで法線表示 #open3d.draw_geometries([pointcloud]) #法線をnumpyへ変換 normals = np.asarray(pointcloud.normals) return normals
def NormalEstimate(points): #objファイルから点群取得 # points = loadOBJ(path) print("points:{}".format(points.shape[0])) #点群をnp配列⇒open3d形式に pointcloud = open3d.PointCloud() pointcloud.points = open3d.Vector3dVector(points) # 法線推定 open3d.estimate_normals(pointcloud, search_param=open3d.KDTreeSearchParamHybrid( radius=5, max_nn=30)) # 法線の方向を視点ベースでそろえる open3d.orient_normals_towards_camera_location(pointcloud, camera_location=np.array( [0., 10., 10.], dtype="float64")) #nキーで法線表示 #open3d.draw_geometries([pointcloud]) #法線をnumpyへ変換 normals = np.asarray(pointcloud.normals) #OBB生成 #(最適化の条件にも使いたい) # _, _, length = buildOBB(points) # print("OBB_length: {}".format(length)) return normals
def _preprocessPointCloud(self, pcd, voxelSize): print("Downsample with a voxel size %.3f." % voxelSize) # pcd_down = pcd.voxel_down_sample(voxelSize) pcd_down = open3d.voxel_down_sample(pcd, voxelSize) # cl, pcd_out_remove = open3d.statistical_outlier_removal(pcd_down, nb_neighbors=20, std_ratio=2.0) radius_normal = voxelSize * 2 print("Estimate normal with search radius %.3f." % radius_normal) # pcd_down.estimate_normals(open3d.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30)) open3d.estimate_normals(pcd_down, open3d.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30)) radius_feature = voxelSize * 5 print(":: Compute FPFH feature with search radius %.3f." % radius_feature) pcd_fpfh = open3d.compute_fpfh_feature( pcd_down,open3d.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100)) return pcd_down, pcd_fpfh
def post_process(affordance_map, class_pred, color_input, color_bg, depth_input, depth_bg, camera_intrinsic): ## scale the images to the proper value color_input = np.asarray(color_input, dtype=np.float32) / 255 color_bg = np.asarray(color_bg, dtype=np.float32) / 255 depth_input = np.asarray(depth_input, dtype=np.float64) / 10000 depth_bg = np.asarray(depth_bg, dtype=np.float64) / 10000 ## get foreground mask frg_mask_color = ~(np.sum(abs(color_input-color_bg) < 0.3, axis=2) == 3) frg_mask_depth = np.logical_and((abs(depth_input-depth_bg) > 0.02), (depth_bg != 0)) foreground_mask = np.logical_or(frg_mask_color, frg_mask_depth) ## project depth to camera space pix_x, pix_y = np.meshgrid(np.arange(640), np.arange(480)) cam_x = (pix_x - camera_intrinsic[0][2]) * depth_input/camera_intrinsic[0][0] cam_y = (pix_y - camera_intrinsic[1][2]) * depth_input/camera_intrinsic[1][1] cam_z = depth_input depth_valid = (np.logical_and(foreground_mask, cam_z) != 0) input_points = np.array([cam_x[depth_valid], cam_y[depth_valid], cam_z[depth_valid]]).transpose() ## get the foreground point cloud pcd = open3d.PointCloud() pcd.points = open3d.Vector3dVector(input_points) open3d.geometry.estimate_normals(pcd, search_param=open3d.KDTreeSearchParamHybrid(radius = 0.1, max_nn = 50) ) ## flip normals to point towards camera open3d.geometry.orient_normals_towards_camera_location(pcd, np.array([0.,0.,0.])) pcd_normals = np.asarray(pcd.normals) ## reproject the normals back to image plane pix_x = np.round((input_points[:,0] * camera_intrinsic[0][0] / input_points[:,2] + camera_intrinsic[0][2])) pix_y = np.round((input_points[:,1] * camera_intrinsic[1][1] / input_points[:,2] + camera_intrinsic[1][2])) surface_normals_map = np.zeros(color_input.shape) n = 0 for n, (x,y) in enumerate(zip(pix_x, pix_y)): x,y = int(x), int(y) surface_normals_map[y,x,0] = pcd_normals[n,0] surface_normals_map[y,x,1] = pcd_normals[n,1] surface_normals_map[y,x,2] = pcd_normals[n,2] ## Compute standard deviation of local normals (baseline) mean_std_norms = np.mean(stdev_filter(surface_normals_map, 25), axis=2) baseline_score = 1 - mean_std_norms/mean_std_norms.max() ## Set affordance to 0 for regions with high surface normal variance affordance_map[baseline_score < 0.1] = 0 affordance_map[~foreground_mask] = 0 class_pred[baseline_score < 0.1] = 0 class_pred[~foreground_mask] = 0 affordance_map[~class_pred.astype(np.bool)] = 0 affordance_map = gaussian_filter(affordance_map, 4) return surface_normals_map, affordance_map, class_pred
def preprocess_point_cloud(pointcloud, voxel_size): keypoints = open3d.voxel_down_sample(pointcloud, voxel_size) radius_normal = voxel_size * 2 view_point = np.array([0., 10., 10.], dtype="float64") open3d.estimate_normals(keypoints, search_param=open3d.KDTreeSearchParamHybrid( radius=radius_normal, max_nn=30)) open3d.orient_normals_towards_camera_location(keypoints, camera_location=view_point) radius_feature = voxel_size * 5 fpfh = open3d.compute_fpfh_feature( keypoints, search_param=open3d.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100)) return keypoints, fpfh
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 _estimate_pointcloud_normals_unorganized(points): assert points.shape[1] == 3 nonnan = ~np.isnan(points).any(axis=1) points_open3d = open3d.PointCloud() points_open3d.points = open3d.Vector3dVector(points[nonnan]) open3d.estimate_normals( points_open3d, search_param=open3d.KDTreeSearchParamHybrid(radius=0.1, max_nn=30), ) return np.asarray(points_open3d.normals)
def predict(color_input, color_bg, depth_input, depth_bg, camera_intrinsic): ## scale the images to the proper value color_input = np.asarray(color_input, dtype=np.float32) / 255 color_bg = np.asarray(color_bg, dtype=np.float32) / 255 depth_input = np.asarray(depth_input, dtype=np.float64) / 10000 depth_bg = np.asarray(depth_bg, dtype=np.float64) / 10000 ## get foreground mask frg_mask_color = ~(np.sum(abs(color_input-color_bg) < 0.3, axis=2) == 3) frg_mask_depth = np.logical_and((abs(depth_input-depth_bg) > 0.02), (depth_bg != 0)) foreground_mask = np.logical_or(frg_mask_color, frg_mask_depth) ## project depth to camera space pix_x, pix_y = np.meshgrid(np.arange(640), np.arange(480)) cam_x = (pix_x - camera_intrinsic[0][2]) * depth_input/camera_intrinsic[0][0] cam_y = (pix_y - camera_intrinsic[1][2]) * depth_input/camera_intrinsic[1][1] cam_z = depth_input depth_valid = (np.logical_and(foreground_mask, cam_z) != 0) input_points = np.array([cam_x[depth_valid], cam_y[depth_valid], cam_z[depth_valid]]).transpose() ## get the foreground point cloud pcd = open3d.PointCloud() pcd.points = open3d.Vector3dVector(input_points) open3d.estimate_normals(pcd, search_param=open3d.KDTreeSearchParamHybrid(radius = 0.1, max_nn = 50)) pcd_normals = np.asarray(pcd.normals) ## flip normals to point towards sensor center = [0,0,0] for k in range(input_points.shape[0]): p1 = center - input_points[k][:] p2 = pcd_normals[k][:] x = np.cross(p1,p2) angle = np.arctan2(np.sqrt((x*x).sum()), p1.dot(p2.transpose())) if (angle > -np.pi/2 and angle < np.pi/2): pcd_normals[k][:] = -pcd_normals[k][:] ## reproject the normals back to image plane pix_x = np.round((input_points[:,0] * camera_intrinsic[0][0] / input_points[:,2] + camera_intrinsic[0][2])) pix_y = np.round((input_points[:,1] * camera_intrinsic[1][1] / input_points[:,2] + cam_intrinsic[1][2])) surface_normals_map = np.zeros(color_input.shape) for n, (x,y) in enumerate(zip(pix_x, pix_y)): x,y = int(x), int(y) surface_normals_map[y,x,0] = pcd_normals[n,0] surface_normals_map[y,x,1] = pcd_normals[n,1] surface_normals_map[y,x,2] = pcd_normals[n,2] ## Compute standard deviation of local normals mean_std_norms = np.mean(stdev_filter(surface_normals_map, 25), axis=2) affordance_map = 1 - mean_std_norms/mean_std_norms.max() affordance_map[~depth_valid] = 0 return surface_normals_map, affordance_map
def estimate_normals_open3d(self, neighborhood, search_radius): """ Estimate normals based on PCA, because we cannot vectorize we rely on the c++ open3d backend """ pcd_normal = open3d.PointCloud() pcd_normal.points = open3d.Vector3dVector(copy.deepcopy(self.points)) open3d.estimate_normals( pcd_normal, open3d.KDTreeSearchParamHybrid(radius=search_radius, max_nn=neighborhood)) self.normals = numpy.asarray(pcd_normal.normals) return
def visualize_trajectory(numDirs=8, dists=[.1, .3, .5], steps=10): dino = DinoAgent() center = dino.center print("dino is located at: " + str(center)) offset_vals = [-.1, .1] offsets = [np.array([i, j, k]) for i in offset_vals for j in offset_vals for k in offset_vals] + [np.array([0, 0, 0])] centers = [np.array(center) + offset for offset in offsets] ptcloud, sc, start_view = dino.visualize_ptcloud('greedy', 80) print("start view is: " + str(start_view)) params = getCandidateParams(start_view, numDirs, centers, dists) print("params: " + str(params)) cand_view_list = [dirToView(d, start_view, c, dist) for (d, c, dist) in params] cand_traj_pts = cand_view_list for (d, c, dist) in params: print("param: " + str((d, c, dist))) int_pts = interpolateTarget(start_view, d, steps, dist, c) pts_list = [int_pts.get() for i in range(int_pts.qsize())] print("points: " + str(pts_list)) cand_traj_pts += pts_list (greedy_dir, greedy_c, greedy_dist) = chooseGreedyAction(start_view, dists, numDirs, sc, centers) print("greedy params: d {}, c {}, dist {}".format(greedy_dir, greedy_c, greedy_dist)) greedy_traj = interpolateTarget(start_view, greedy_dir, steps, greedy_dist, greedy_c) greedy_pts_list = [greedy_traj.get() for i in range(greedy_traj.qsize())] pcd = open3d.geometry.PointCloud() pcd.points = open3d.Vector3dVector(ptcloud[:, 0:3]) pcd.paint_uniform_color([1,0.706,0]) total_ptcloud = np.vstack((ptcloud[:, 0:3], np.array(cand_traj_pts)[:, 0:3])) tpcd = open3d.geometry.PointCloud() tpcd.points = open3d.Vector3dVector(total_ptcloud) open3d.estimate_normals(tpcd, search_param = open3d.KDTreeSearchParamHybrid(radius = 0.1, max_nn = 100)) open3d.orient_normals_to_align_with_direction(tpcd) tpcd.paint_uniform_color([1,0.706,0]) greedy_pcd = open3d.geometry.PointCloud() greedy_pcd.points = open3d.Vector3dVector(np.array(greedy_pts_list)[:, 0:3]) greedy_pcd.paint_uniform_color([0, 1, 1]) traj_pcd = open3d.geometry.PointCloud() traj_pcd.points = open3d.Vector3dVector(np.array(cand_traj_pts)[:, 0:3]) traj_pcd.paint_uniform_color([1,0.706,0]) open3d.visualization.draw_geometries([pcd, greedy_pcd]) open3d.write_point_cloud("trajectory_viz.ply", tpcd)
def depth_image_to_pointcloud(img, filter = [None, None, None], estimate_normals=True): depth_image_preprocessed = preprocess_image(img) mat = depth_image_to_scientific_coordinates(depth_image_preprocessed, flatten=True, filter=filter) if mat.shape[0] == 0: print('No points left after filtering.') return None mat = scientific_coordinates_to_standard(mat) print_pointcloud_mat_stats(mat) pcd = open3d.PointCloud() pcd.points = open3d.Vector3dVector(mat) if estimate_normals: open3d.estimate_normals(pcd, search_param=open3d.KDTreeSearchParamHybrid(radius=0.1, max_nn=30)) return pcd
def draw_normal(point_cloud): """ :param point_cloud: :return: """ if isinstance(point_cloud, PointCloud): tmp = point_cloud source = o3d.PointCloud() source.points = o3d.Vector3dVector(tmp.position) result = o3d.estimate_normals( source, o3d.KDTreeSearchParamHybrid(radius=50, max_nn=9)) print('result is', result, 'result type is', type(result))
def load_pcd(data_path, cat): # load meshes ply_path = os.path.join(data_path, 'meshes', 'obj_' + cat + '.ply') model_vsd = ply_loader.load_ply(ply_path) pcd_model = open3d.PointCloud() pcd_model.points = open3d.Vector3dVector(model_vsd['pts']) open3d.estimate_normals(pcd_model, search_param=open3d.KDTreeSearchParamHybrid( radius=0.1, max_nn=30)) # open3d.draw_geometries([pcd_model]) model_vsd_mm = copy.deepcopy(model_vsd) model_vsd_mm['pts'] = model_vsd_mm['pts'] * 1000.0 #pcd_model = open3d.read_point_cloud(ply_path) return pcd_model, model_vsd, model_vsd_mm
def merge_pointclouds(pcd1, pcd2, keep_colors=True, estimate_normals=True): if pcd1 is None and pcd2 is None: return None if pcd1 is None: return pcd2 if pcd2 is None: return pcd1 pcd = open3d.PointCloud() pcd.points = open3d.Vector3dVector(np.vstack((np.asarray(pcd1.points), np.asarray(pcd2.points)))) if keep_colors: pcd.colors = open3d.Vector3dVector(np.vstack((np.asarray(pcd1.colors), np.asarray(pcd2.colors)))) if estimate_normals: open3d.estimate_normals(pcd, search_param=open3d.KDTreeSearchParamHybrid(radius=0.1, max_nn=30)) return pcd
def registration_point_to_plane(scene1, scene2, voxel_size): # scene1 and 2 are point cloud data # voxel_size is grid size #draw_registration_result(scene1,scene2,np.identity(4)) # voxel down sampling scene1_down = open3d.voxel_down_sample(scene1, voxel_size) scene2_down = open3d.voxel_down_sample(scene2, voxel_size) #draw_registration_result(scene1_down,scene2_down,np.identity(4)) # estimate normal with search radius voxel_size*2.0 radius_normal = voxel_size * 2.0 open3d.estimate_normals( scene1, open3d.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30)) open3d.estimate_normals( scene2, open3d.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30)) open3d.estimate_normals( scene1_down, open3d.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30)) open3d.estimate_normals( scene2_down, open3d.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30)) # compute fpfh feature with search radius voxel_size/2.0 radius_feature = voxel_size * 2.0 scene1_fpfh = open3d.compute_fpfh_feature( scene1_down, search_param=open3d.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100)) scene2_fpfh = open3d.compute_fpfh_feature( scene2_down, search_param=open3d.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100)) # compute ransac registration ransac_result = execute_global_registration(scene1_down, scene2_down, scene1_fpfh, scene2_fpfh, voxel_size) #draw_registration_result(scene1,scene2,ransac_result.transformation) # point to point ICP resigtration distance_threshold = voxel_size * 10.0 result = open3d.registration_icp( scene1, scene2, distance_threshold, ransac_result.transformation, open3d.TransformationEstimationPointToPlane(), open3d.ICPConvergenceCriteria(max_iteration=1000)) #draw_registration_result(scene1,scene2,result.transformation) print(result) return result
def load_pcd(cat): # load meshes mesh_path = "/home/sthalham/data/Meshes/linemod_13/" #mesh_path = "/home/stefan/data/val_linemod_cc_rgb/models_ply/" ply_path = mesh_path + 'obj_' + cat + '.ply' model_vsd = ply_loader.load_ply(ply_path) pcd_model = open3d.PointCloud() pcd_model.points = open3d.Vector3dVector(model_vsd['pts']) open3d.estimate_normals(pcd_model, search_param=open3d.KDTreeSearchParamHybrid( radius=0.1, max_nn=30)) # open3d.draw_geometries([pcd_model]) model_vsd_mm = copy.deepcopy(model_vsd) model_vsd_mm['pts'] = model_vsd_mm['pts'] * 1000.0 pcd_model = open3d.read_point_cloud(ply_path) return pcd_model, model_vsd, model_vsd_mm
def estimate_normals_open3d(pcd, k=1): dummy = copy.deepcopy(pcd) mean_nn = pyreg.functions.get_mean_nn_distance(dummy) voxel_size = k * mean_nn pcd_open3d = open3d.PointCloud() pcd_open3d.points = open3d.Vector3dVector(dummy.points) pcd_open3d = open3d.voxel_down_sample(pcd_open3d, voxel_size=voxel_size) radius_normal = 2 * voxel_size open3d.estimate_normals( pcd_open3d, open3d.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30)) pcd = PointCloud(points=numpy.asarray(pcd_open3d.points), normals=numpy.asarray(pcd_open3d.normals)) return pcd
def load_pcd(cat): # load meshes #mesh_path = "/home/sthalham/data/LINEMOD/models/" mesh_path = "/home/stefan/data/Meshes/ycb_video/models/" template = '000000' lencat = len(cat) cat = template[:-lencat] + cat ply_path = mesh_path + 'obj_' + cat + '.ply' model_vsd = ply_loader.load_ply(ply_path) pcd_model = open3d.PointCloud() pcd_model.points = open3d.Vector3dVector(model_vsd['pts']) open3d.estimate_normals(pcd_model, search_param=open3d.KDTreeSearchParamHybrid( radius=20.0, max_nn=30)) # open3d.draw_geometries([pcd_model]) model_vsd_mm = copy.deepcopy(model_vsd) model_vsd_mm['pts'] = model_vsd_mm['pts'] * 1000.0 pcd_model = open3d.read_point_cloud(ply_path) return pcd_model, model_vsd, model_vsd_mm
def extract_fpfh(self, radius, max_nn): """ Extract the FastPointFeatureHistograms of a point cloud with Open3D args: radius: radius for nn-search. Use a much higher radius than for normal computation! e.g. 10xmean_nn max_nn: max number of nearest neighbors within the radius. Use a higher value as well! e.g. 100 """ pcd_open3d = open3d.PointCloud() pcd_open3d.points = open3d.Vector3dVector(self.points) if self.has_normals() == True: pcd_open3d.normals = open3d.Vector3dVector(self.normals) else: raise Exception("Compute normals before computing FPFH!") fpfh = open3d.compute_fpfh_feature( pcd_open3d, open3d.KDTreeSearchParamHybrid(radius=radius, max_nn=max_nn)) self.fpfh = numpy.asarray(fpfh.data).T return