# 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
