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)
