def extract_red_alphashape(cloud, robot):
"""
extract red, get alpha shape, downsample
"""
raise NotImplementedError
# downsample cloud
cloud_ds = cloudprocpy.downsampleCloud(cloud, .01)
# transform into body frame
xyz1_kinect = cloud_ds.to2dArray()
xyz1_kinect[:, 3] = 1
T_w_k = berkeley_pr2.get_kinect_transform(robot)
xyz1_robot = xyz1_kinect.dot(T_w_k.T)
# compute 2D alpha shape
xyz1_robot_flat = xyz1_robot.copy()
xyz1_robot_flat[:, 2] = 0 # set z coordinates to zero
xyz1_robot_flatalphashape = cloudprocpy.computeAlphaShape(xyz1_robot_flat)
# unfortunately pcl alpha shape func throws out the indices, so we have to use nearest neighbor search
cloud_robot_flatalphashape = cloudprocpy.CloudXYZ()
cloud_robot_flatalphashape.from2dArray(xyz1_robot_flatalphashape)
cloud_robot_flat = cloudprocpy.CloudXYZ()
cloud_robot_flat.from2dArray(xyz1_robot_flat)
alpha_inds = cloudprocpy.getNearestNeighborIndices(
xyz1_robot_flatalphashape, xyz1_robot_flat)
xyz_robot_alphashape = xyz1_robot_flatalphashape[:, :3]
# put back z coordinate
xyz_robot_alphashape[:, 2] = xyz1_robot[alpha_inds, 2]
return xyz_robot_alphashape[:, :3]
def extract_red_alphashape(cloud, robot):
"""
extract red, get alpha shape, downsample
"""
raise NotImplementedError
# downsample cloud
cloud_ds = cloudprocpy.downsampleCloud(cloud, .01)
# transform into body frame
xyz1_kinect = cloud_ds.to2dArray()
xyz1_kinect[:,3] = 1
T_w_k = berkeley_pr2.get_kinect_transform(robot)
xyz1_robot = xyz1_kinect.dot(T_w_k.T)
# compute 2D alpha shape
xyz1_robot_flat = xyz1_robot.copy()
xyz1_robot_flat[:,2] = 0 # set z coordinates to zero
xyz1_robot_flatalphashape = cloudprocpy.computeAlphaShape(xyz1_robot_flat)
# unfortunately pcl alpha shape func throws out the indices, so we have to use nearest neighbor search
cloud_robot_flatalphashape = cloudprocpy.CloudXYZ()
cloud_robot_flatalphashape.from2dArray(xyz1_robot_flatalphashape)
cloud_robot_flat = cloudprocpy.CloudXYZ()
cloud_robot_flat.from2dArray(xyz1_robot_flat)
alpha_inds = cloudprocpy.getNearestNeighborIndices(xyz1_robot_flatalphashape, xyz1_robot_flat)
xyz_robot_alphashape = xyz1_robot_flatalphashape[:,:3]
# put back z coordinate
xyz_robot_alphashape[:,2] = xyz1_robot[alpha_inds,2]
return xyz_robot_alphashape[:,:3]
def observe_cloud(self):
if self.T_w_k is None:
if self.robot is None:
raise RuntimeError(
"Can't observe cloud when there is no robot")
else:
from lfd.rapprentice import berkeley_pr2
self.T_w_k = berkeley_pr2.get_kinect_transform(self.robot)
# camera's parameters
cx = 320. - .5
cy = 240. - .5
f = 525. # focal length
w = 640.
h = 480.
pixel_ij = np.array(np.meshgrid(np.arange(w), np.arange(h))).T.reshape(
(-1, 2)) # all pixel positions
rays_to = self.range_k * np.c_[(pixel_ij - np.array([cx, cy])) / f,
np.ones(pixel_ij.shape[0])]
rays_from = np.zeros_like(rays_to)
# transform the rays from the camera frame to the world frame
rays_to = rays_to.dot(self.T_w_k[:3, :3].T) + self.T_w_k[:3, 3]
rays_from = rays_from.dot(self.T_w_k[:3, :3].T) + self.T_w_k[:3, 3]
cloud = []
for sim_obj in self.dyn_sim_objs:
for bt_obj in sim_obj.get_bullet_objects():
ray_collisions = self.bt_env.RayTest(rays_from, rays_to,
bt_obj)
pts = np.empty((len(ray_collisions), 3))
for i, ray_collision in enumerate(ray_collisions):
pts[i, :] = ray_collision.pt
cloud.append(pts)
cloud = np.concatenate(cloud)
# hack to filter out point below the top of the table. TODO: fix this hack
table_sim_objs = [
sim_obj for sim_obj in self.sim_objs if "table" in sim_obj.names
]
assert len(table_sim_objs) == 1
table_sim_obj = table_sim_objs[0]
table_height = table_sim_obj.translation[2] + table_sim_obj.extents[2]
cloud = cloud[cloud[:, 2] > table_height, :]
return cloud
def observe_cloud(self):
if self.T_w_k is None:
if self.robot is None:
raise RuntimeError("Can't observe cloud when there is no robot")
else:
from lfd.rapprentice import berkeley_pr2
self.T_w_k = berkeley_pr2.get_kinect_transform(self.robot)
# camera's parameters
cx = 320.-.5
cy = 240.-.5
f = 525. # focal length
w = 640.
h = 480.
pixel_ij = np.array(np.meshgrid(np.arange(w), np.arange(h))).T.reshape((-1, 2)) # all pixel positions
rays_to = self.range_k * np.c_[(pixel_ij - np.array([cx, cy])) / f, np.ones(pixel_ij.shape[0])]
rays_from = np.zeros_like(rays_to)
# transform the rays from the camera frame to the world frame
rays_to = rays_to.dot(self.T_w_k[:3,:3].T) + self.T_w_k[:3,3]
rays_from = rays_from.dot(self.T_w_k[:3,:3].T) + self.T_w_k[:3,3]
cloud = []
for sim_obj in self.dyn_sim_objs:
for bt_obj in sim_obj.get_bullet_objects():
ray_collisions = self.bt_env.RayTest(rays_from, rays_to, bt_obj)
pts = np.empty((len(ray_collisions), 3))
for i, ray_collision in enumerate(ray_collisions):
pts[i, :] = ray_collision.pt
cloud.append(pts)
cloud = np.concatenate(cloud)
# hack to filter out point below the top of the table. TODO: fix this hack
table_sim_objs = [sim_obj for sim_obj in self.sim_objs if "table" in sim_obj.names]
assert len(table_sim_objs) == 1
table_sim_obj = table_sim_objs[0]
table_height = table_sim_obj.translation[2] + table_sim_obj.extents[2]
cloud = cloud[cloud[:, 2] > table_height, :]
return cloud
