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 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 _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 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 _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 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 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
def preprocess_point_cloud(): """ Pre-process point cloud - downsample, estimate normals and calculate fpfh features Based on http://www.open3d.org/docs/tutorial/Advanced/global_registration.html#extract-geometric-feature """ node = hou.pwd() node_geo = node.geometry() downsample = node.parm("downsample").eval() voxel_size = node.parm("voxel_size").eval() estimate_normals = node.parm("estimate_normals").eval() max_neighbours = node.parm("max_neighbours").eval() compute_fpfh_feature = node.parm("compute_fpfh_feature").eval() max_neighbours_fpfh = node.parm("max_neighbours_fpfh").eval() # check for attributes has_n = bool(node_geo.findPointAttrib("N")) has_cd = bool(node_geo.findPointAttrib("Cd")) # to numpy np_pos_str = node_geo.pointFloatAttribValuesAsString("P", float_type=hou.numericData.Float32) np_pos = np.fromstring(np_pos_str, dtype=np.float32).reshape(-1, 3) # to open3d pcd = open3d.PointCloud() pcd.points = open3d.Vector3dVector(np_pos.astype(np.float64)) if has_n: np_n_str = node_geo.pointFloatAttribValuesAsString("N", float_type=hou.numericData.Float32) np_n = np.fromstring(np_n_str, dtype=np.float32).reshape(-1, 3) pcd.normals = open3d.Vector3dVector(np_n.astype(np.float64)) if has_cd: np_cd_str = node_geo.pointFloatAttribValuesAsString("Cd", float_type=hou.numericData.Float32) np_cd = np.fromstring(np_cd_str, dtype=np.float32).reshape(-1, 3) pcd.colors = open3d.Vector3dVector(np_cd.astype(np.float64)) # preprocess if downsample: pcd = open3d.voxel_down_sample(pcd, voxel_size) if estimate_normals: open3d.estimate_normals(pcd, open3d.KDTreeSearchParamHybrid(radius=voxel_size*2, max_nn=max_neighbours)) pcd_fpfh = None if compute_fpfh_feature: pcd_fpfh = open3d.compute_fpfh_feature(pcd, open3d.KDTreeSearchParamHybrid(radius=voxel_size*5, max_nn=max_neighbours_fpfh)) np_fpfh = np.asarray(pcd_fpfh.data) np_fpfh = np.swapaxes(np_fpfh, 1, 0) # to houdini np_pos = np.asarray(pcd.points) np_n = np.asarray(pcd.normals) np_cd = np.asarray(pcd.colors) if downsample: node_geo.deletePoints(node_geo.points()) node_geo.createPoints(np_pos) else: node_geo.setPointFloatAttribValuesFromString("P", np_pos, float_type=hou.numericData.Float64) if has_n or estimate_normals: if not has_n: node_geo.addAttrib(hou.attribType.Point, "N", default_value=(0.0, 0.0, 0.0), transform_as_normal=True, create_local_variable=False) node_geo.setPointFloatAttribValuesFromString("N", np_n, float_type=hou.numericData.Float64) if has_cd: node_geo.setPointFloatAttribValuesFromString("Cd", np_cd, float_type=hou.numericData.Float64) if compute_fpfh_feature: node_geo.addAttrib(hou.attribType.Point, "fpfh", default_value=(0.0, )*np_fpfh.shape[1], transform_as_normal=False, create_local_variable=False) node_geo.setPointFloatAttribValuesFromString("fpfh", np_fpfh, float_type=hou.numericData.Float64)