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 = []
atexit.register(rave.RaveDestroy)
#env.SetViewer('qtcoin')
robot = env.GetRobots()[0]
robot.SetDOFValues(np.zeros(robot.GetDOF()))
robot.SetTransform(np.eye(4))
rgj = robot.GetJoint("r_gripper_l_finger_joint")
robot.SetDOFValues([rgj.GetLimits()[-1]], [rgj.GetDOFIndex()])
tlj = robot.GetJoint("torso_lift_joint")
robot.SetDOFValues([tlj.GetLimits()[-1]], [tlj.GetDOFIndex()])
print "processing point cloud"
if env.GetKinBody("convexsoup") is None:
cloud = cloudprocpy.readPCDXYZ("/home/joschu/Proj/trajoptrave/bigdata/laser_cloud.pcd")
cloud = cloudprocpy.boxFilter(cloud, -1,5,-5,5,.1,2)
aabb = robot.GetLink("base_link").ComputeAABB()
(xmin,ymin,zmin) = aabb.pos() - aabb.extents()
(xmax,ymax,zmax) = aabb.pos() + aabb.extents()
cloud = cloudprocpy.boxFilterNegative(cloud, xmin,xmax,ymin,ymax,zmin,zmax)
cloud = cloudprocpy.downsampleCloud(cloud, .015)
convex_soup.create_convex_soup(cloud, env)
print "done processing"
T = np.eye(4)
T[:3,3] = [ 1.27 , -0.2 , 0.790675]
##################
