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 downsample_colored(xyzrgb, v):
import cloudprocpy
cloud = cloudprocpy.CloudXYZ()
xyz1 = np.ones((len(xyzrgb),4),'float')
xyz1[:,:3] = xyzrgb[:,:3]
cloud.from2dArray(xyz1)
cloud_downsampled = cloudprocpy.downsampleCloud(cloud, v)
indices = cloudprocpy.getNearestNeighborIndices(cloud_downsampled, cloud)
return np.hstack((cloud_downsampled.to2dArray()[:,:3], xyzrgb[indices,3:]))
