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:]))