예제 #1
0
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
예제 #2
0
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
예제 #3
0
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 = []