# BEGIN generate_mesh def generate_mesh(cloud): cloud = cloudprocpy.fastBilateralFilter(cloud, 15, .05) # smooth out the depth image cloud = remove_floor(cloud) # remove points with low height (in world frame) big_mesh = cloudprocpy.meshOFM(cloud, 3, .1) # use pcl OrganizedFastMesh to make mesh simple_mesh = cloudprocpy.quadricSimplifyVTK(big_mesh, .02) # decimate mesh with VTK function return simple_mesh # END generate_mesh def get_xyz_world_frame(cloud): xyz1 = cloud.to2dArray() xyz1[:,3] = 1 return xyz1.dot(T_w_k.T)[:,:3] cloud_orig = cloudprocpy.readPCDXYZ(osp.join(trajoptpy.bigdata_dir,args.scene_name,"cloud.pcd")) dof_vals = np.loadtxt(osp.join(trajoptpy.bigdata_dir, args.scene_name, "dof_vals.txt")) T_w_k = np.loadtxt(osp.join(trajoptpy.bigdata_dir, args.scene_name, "kinect_frame.txt")) qw,qx,qy,qz,px,py,pz = np.loadtxt(osp.join(trajoptpy.bigdata_dir, args.scene_name, "pose_target.txt")) env = openravepy.Environment() env.StopSimulation() env.Load("robots/pr2-beta-static.zae") robot = env.GetRobots()[0] robot.SetDOFValues(dof_vals) viewer = trajoptpy.GetViewer(env) handles = [] try:
grabber=cloudprocpy.CloudGrabber() xyzrgb = None xyzrgb = grabber.getXYZRGB() #if not args.dry_run: xyzrgb.save(osp.join(scene_dir,"cloud.pcd")) #time.sleep(5) print "Before making mesh" if xyzrgb != None: print "Point cloud from grabber" xyzrgb.save("cloud.pcd") point_cloud = cloudprocpy.readPCDXYZ("cloud.pcd") """ elif (point_cloud == None) and (point_cloud_2 == None): print "No point cloud" elif point_cloud_2 != None: point_cloud = point_cloud_2 """ #print "Point Cloud ROS: " #print point_cloud_2 print "Cloud Grabber: " print xyzrgb #cloud_orig = cloudprocpy.readPCDXYZ("cloud.pcd") mesh = generate_mesh(point_cloud) mesh_body = mk.create_trimesh(env, get_xyz_world_frame(mesh.getCloud()), np.array(mesh.getFaces()), name="simple_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 = []
cloud, 3, .1) # use pcl OrganizedFastMesh to make mesh simple_mesh = cloudprocpy.quadricSimplifyVTK( big_mesh, .02) # decimate mesh with VTK function return simple_mesh # END generate_mesh def get_xyz_world_frame(cloud): xyz1 = cloud.to2dArray() xyz1[:, 3] = 1 return xyz1.dot(T_w_k.T)[:, :3] cloud_orig = cloudprocpy.readPCDXYZ( osp.join(trajoptpy.bigdata_dir, args.scene_name, "cloud.pcd")) dof_vals = np.loadtxt( osp.join(trajoptpy.bigdata_dir, args.scene_name, "dof_vals.txt")) T_w_k = np.loadtxt( osp.join(trajoptpy.bigdata_dir, args.scene_name, "kinect_frame.txt")) qw, qx, qy, qz, px, py, pz = np.loadtxt( osp.join(trajoptpy.bigdata_dir, args.scene_name, "pose_target.txt")) env = openravepy.Environment() env.StopSimulation() env.Load("robots/pr2-beta-static.zae") robot = env.GetRobots()[0] robot.SetDOFValues(dof_vals) viewer = trajoptpy.GetViewer(env)
big_mesh = cloudprocpy.meshOFM(cloud, 3, .1) # use pcl OrganizedFastMesh to make mesh simple_mesh = cloudprocpy.quadricSimplifyVTK(big_mesh, .02) # decimate mesh with VTK function return simple_mesh env = openravepy.Environment() env.StopSimulation() filenames = [] for root, dirs, files in os.walk('/home/annal/src/RavenDebridement/pc_data/'): for f in files: if f.endswith('.pcd'): filenames.append(f) break filenames.sort() cloud_orig = cloudprocpy.readPCDXYZ(osp.join('/home/annal/src/RavenDebridement/pc_data/', filenames[len(files)-1])) mesh = generate_mesh(cloud_orig) mesh_body = mk.create_trimesh(env, get_xyz_world_frame(mesh.getCloud()), np.array(mesh.getFaces()), name="simple_mesh") viewer = trajoptpy.GetViewer(env) cloud_orig_colored = cloudprocpy.readPCDXYZRGB(osp.join('/home/annal/src/RavenDebridement/pc_data/', filenames[len(files)-1])) rgbfloats = cloud_orig_colored.to2dArray()[:,4] rgb0 = np.ndarray(buffer=rgbfloats.copy(),shape=(480*640,4),dtype='uint8') xyz=get_xyz_world_frame(cloud_orig) goodinds = np.isfinite(xyz[:,0]) cloud_handle = env.plot3(xyz[goodinds,:], 2,(rgb0[goodinds,:3][:,::-1])/255. ) viewer.Idle() del cloud_handle