def generate_mesh(cloud, decimation_rate=0.5):
"""
For unordered point cloud, calculates normals and then creates mesh
:param cloud: cloudprocpy cloud
:return pcl polygon mesh
"""
# import warnings
# warnings.filterwarnings('error')
cloud_with_normals = cloudprocpy.mlsAddNormals(cloud, .02)
filtered_cloud = list()
for pt_and_normal in cloud_with_normals.to2dArray():
normal = pt_and_normal[3:]
# try:
if np.linalg.norm(normal) < 1e4:
filtered_cloud.append(pt_and_normal)
# except Warning as e:
# import IPython
# IPython.embed()
cloud_with_normals.from2dArray(np.array(filtered_cloud))
big_mesh = cloudprocpy.meshGP3(cloud_with_normals, search_radius=0.02) # 0.04
# simple_mesh = cloudprocpy.quadricSimplifyVTK(big_mesh, decimation_rate) # decimate mesh with VTK function
# return simple_mesh
return big_mesh
def generate_mesh(cloud, decimation_rate=0.5):
"""
For unordered point cloud, calculates normals and then creates mesh
:param cloud: cloudprocpy cloud
:return pcl polygon mesh
"""
# import warnings
# warnings.filterwarnings('error')
cloud_with_normals = cloudprocpy.mlsAddNormals(cloud, .02)
filtered_cloud = list()
for pt_and_normal in cloud_with_normals.to2dArray():
normal = pt_and_normal[3:]
# try:
if np.linalg.norm(normal) < 1e4:
filtered_cloud.append(pt_and_normal)
# except Warning as e:
# import IPython
# IPython.embed()
cloud_with_normals.from2dArray(np.array(filtered_cloud))
big_mesh = cloudprocpy.meshGP3(cloud_with_normals, search_radius=0.02) # 0.04
# simple_mesh = cloudprocpy.quadricSimplifyVTK(big_mesh, decimation_rate) # decimate mesh with VTK function
# return simple_mesh
return big_mesh
import numpy as np
import cloudprocpy
cloud = cloudprocpy.readPCDXYZ("/home/joschu/Proj/trajoptrave/bigdata/laser_cloud.pcd")
cloud = cloudprocpy.downsampleCloud(cloud, .01)
cloud = cloudprocpy.boxFilter(cloud, .5,5,-3,3,.1,2.5)
cloud.save("/tmp/cloud.ply")
xyzorig = cloud.to2dArray()[:,:3]
xyzn = cloudprocpy.mlsAddNormals(cloud).to2dArray()
xyz = xyzn[:,:3]
normals = xyzn[:,4:7]
from trajoptpy import math_utils
normals = math_utils.normr(normals)
camerapos = np.array([0,0,1.3])
ndotp = ((camerapos[None,:] - xyz) * normals).sum(axis=1)
normals *= np.sign(ndotp)[:,None]
import trajoptpy, openravepy
env = openravepy.RaveGetEnvironment(1)
if env is None:
env = openravepy.Environment()
viewer = trajoptpy.GetViewer(env)
import IPython
IPython.lib.inputhook.set_inputhook(viewer.Step)
xyzn[:,4:7] = normals
cloudwn = cloudprocpy.CloudXYZN()
cloudwn.from2dArray(xyzn)
cloudwn.save("/tmp/lasercloud.ply")
handles = []
