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(xyz, v):
cloud = cloudprocpy.CloudXYZ()
xyz1 = np.ones((len(xyz), 4), 'float')
xyz1[:, :3] = xyz
cloud.from2dArray(xyz1)
cloud = cloudprocpy.downsampleCloud(cloud, v)
return cloud.to2dArray()[:, :3]
def pc2_to_cloudprocpy(pc2, transform_world, point_cloud_filter):
"""
:param pc2: the point cloud to read from
:type pc2: sensor_msgs.PointCloud2
:param cam_pose_world: 4x4 np.ndarray of transform for cloud in frame world
"""
points_gen = read_points(pc2, skip_nans=False)
rot = transform_world[:3,:3]
trans = transform_world[:3,3]
points = list()
for pt in points_gen:
if point_cloud_filter(pt):
pt = list(np.dot(rot, pt) + trans)
pt.append(1)
points.append(pt)
# reorganize cloud data to construct a cloud object
n_dim = 4 # 3 spatial dimension + 1 for homogenity
cloud = cloudprocpy.CloudXYZ()
xyz1 = np.array(points)
cloud.from2dArray(xyz1)
return cloud
def downsample(xyz, v):
import cloudprocpy
if xyz.shape[1] == 3:
cloud = cloudprocpy.CloudXYZ()
xyz1 = np.ones((len(xyz), 4), 'float')
xyz1[:, :3] = xyz
cloud.from2dArray(xyz1)
cloud = cloudprocpy.downsampleCloud(cloud, v)
return cloud.to2dArray()[:, :3]
else:
# rgb fields needs to be packed and upacked as described in here
# http://docs.pointclouds.org/1.7.0/structpcl_1_1_point_x_y_z_r_g_b.html
xyzrgb = xyz
n = xyzrgb.shape[0]
cloud = cloudprocpy.CloudXYZRGB()
xyzrgb1 = np.ones((n, 8), 'float')
xyzrgb1[:, :3] = xyzrgb[:, :3]
xyzrgb1[:, 4] = cloudprocpy.packRGBs(xyzrgb[:, 3:] * 255.0)
xyzrgb1[:, 5:] = np.zeros(
(n, 3)) # padding that is not used. set to zero just in case
cloud.from2dArray(xyzrgb1)
cloud = cloudprocpy.downsampleColorCloud(cloud, v)
xyzrgb1 = cloud.to2dArray()
return np.c_[xyzrgb1[:, :3],
cloudprocpy.unpackRGBs(xyzrgb1[:, 4]) / 255.0]
def kinect_callback(self, cloud_msg):
# convert from point cloud message to cloudprocpy.CloudXYZ
points_gen = self.read_points(cloud_msg, skip_nans=False)
points = []
for pt in points_gen:
pt = list(pt)
pt.append(1)
points.append(pt)
# reorganize cloud data to construct a cloud object
n_dim = 4 # 3 spatial dimension + 1 for homogenity
cloud = cloudprocpy.CloudXYZ()
xyz1 = np.array(points)
xyz1 = np.reshape(xyz1, (raven_constants.Kinect.height, raven_constants.Kinect.width, n_dim))
cloud.from3dArray(xyz1)
#save cloud for next run
np.save('environment_obstacles_%d'%(self.cloud_id), xyz1)
# add new bodies and cloud first
new_cd_body_names = self.generateBodies(cloud, self.cloud_id)
# remove previous bodies and cloud
for name in self.cd_body_names:
self.env.Remove(self.env.GetKinBody(name))
self.cd_body_names = new_cd_body_names
self.cloud_id = self.cloud_id+1
def remove_outliers(xyz, thresh=2.0, k=15):
import cloudprocpy
cloud = cloudprocpy.CloudXYZ()
xyz1 = np.ones((len(xyz),4),'float')
xyz1[:,:3] = xyz
cloud.from2dArray(xyz1)
cloud = cloudprocpy.removeOutliers(cloud, thresh, k)
return cloud.to2dArray()[:,:3]
def median_filter(xyz, window_size, max_allowed_movement):
import cloudprocpy
cloud = cloudprocpy.CloudXYZ()
xyz1 = np.ones((len(xyz),4),'float')
xyz1[:,:3] = xyz
cloud.from2dArray(xyz1)
cloud = cloudprocpy.medianFilter(cloud, window_size, max_allowed_movement)
return cloud.to2dArray()[:,:3]
def voxelize(self, cloud, voxelSize=0.003):
'''TODO'''
cloud = numpy.ascontiguousarray(
cloud) # cloudprocpy assumes this ordering
n = cloud.shape[0]
augment = zeros((n, 1))
bigCloud = concatenate((cloud, augment), axis=1)
cloudTrajopt = cloudprocpy.CloudXYZ()
cloudTrajopt.from2dArray(bigCloud)
dsCloud = cloudprocpy.downsampleCloud(cloudTrajopt, voxelSize)
cloud = dsCloud.to2dArray()[:, :3]
return cloud
def filterStatisticalOutliers(self, cloud, kNeighbors=50, std=1.0):
'''TODO'''
cloud = numpy.ascontiguousarray(cloud) # cloudprocpy assumes this
n = cloud.shape[0]
augment = zeros((n, 1))
bigCloud = concatenate((cloud, augment), axis=1)
cloudTrajopt = cloudprocpy.CloudXYZ()
cloudTrajopt.from2dArray(bigCloud)
dsCloud = cloudprocpy.statisticalOutlierRemoval(
cloudTrajopt, kNeighbors, std)
cloud = dsCloud.to2dArray()[:, :3]
return cloud
def cluster_filter(xyz, tol=0.08, minsize=200):
import cloudprocpy
cloud = cloudprocpy.CloudXYZ()
xyz1 = np.ones((len(xyz),4),'float')
xyz1[:,:3] = xyz
cloud.from2dArray(xyz1)
while True:
filter_result = cloudprocpy.clusterFilter(cloud, tol, minsize)
filter_result = filter_result.to2dArray()[:,:3]
if len(filter_result) > 0:
cloud = filter_result
break
else:
minsize = int(0.5 * minsize)
return cloud
def extract_red(rgb, depth, T_w_k):
"""
extract red points and downsample
"""
hsv = cv2.cvtColor(rgb, cv2.COLOR_BGR2HSV)
h = hsv[:, :, 0]
s = hsv[:, :, 1]
v = hsv[:, :, 2]
red_mask = ((h < 10) | (h > 150)) & (s > 100) & (v > 100)
valid_mask = depth > 0
xyz_k = clouds.depth_to_xyz(depth, berkeley_pr2.f)
xyz_w = xyz_k.dot(T_w_k[:3, :3].T) + T_w_k[:3, 3][None, None, :]
z = xyz_w[:, :, 2]
z0 = xyz_k[:, :, 2]
if DEBUG_PLOTS:
cv2.imshow("z0", z0 / z0.max())
cv2.imshow("z", z / z.max())
cv2.imshow("rgb", rgb)
cv2.waitKey()
height_mask = xyz_w[:, :, 2] > .7 # XXXX pass in parameter
good_mask = red_mask & height_mask & valid_mask
if DEBUG_PLOTS:
cv2.imshow("red", red_mask.astype('uint8') * 255)
cv2.imshow("above_table", height_mask.astype('uint8') * 255)
cv2.imshow("red and above table", good_mask.astype('uint8') * 255)
print "press enter to continue"
cv2.waitKey()
good_xyz = xyz_w[good_mask]
cloud = cloudprocpy.CloudXYZ()
good_xyz1 = np.zeros((len(good_xyz), 4), 'float32')
good_xyz1[:, :3] = good_xyz
cloud.from2dArray(good_xyz1)
cloud_ds = cloudprocpy.downsampleCloud(cloud, .01)
return cloud_ds.to2dArray()[:, :3]
def computeNormals(self, cloud, viewPoint=None):
'''TODO'''
cloud = numpy.ascontiguousarray(cloud) # cloudprocpy assumes this
augment = zeros((cloud.shape[0], 1))
bigCloud = concatenate((cloud, augment), axis=1)
cloudTrajopt = cloudprocpy.CloudXYZ()
cloudTrajopt.from2dArray(bigCloud)
normals = cloudprocpy.normalEstimation(cloudTrajopt).to2dArray()[:,
4:7]
# reverse any normals going in the wrong direction
if viewPoint is not None:
for i in xrange(len(normals)):
d = viewPoint - cloud[i]
if dot(normals[i], d / norm(d)) < 0:
normals[i] = -normals[i]
return normals
def addCloudToEnvironment(self, cloud, cubeSize=0.02):
'''
Add a point cloud to the OpenRAVE environment as a collision obstacle.
@param cloud: list of 3-tuples, [(x1,y1,z1),...,(xn,yn,zn)]
'''
# remove existing cloud, if there is one
self.removeCloudFromEnvironment()
# convert point cloud to expected format
cloud = ascontiguousarray(cloud) # cloudprocpy assumes this
cloud = concatenate((cloud, zeros((cloud.shape[0], 1))), axis=1)
cloudTrajopt = cloudprocpy.CloudXYZ()
cloudTrajopt.from2dArray(cloud)
# downsample and add to environment
dsCloud = cloudprocpy.downsampleCloud(cloudTrajopt, cubeSize)
self.obstacleCloud = make_kinbodies.create_boxes(
self.env,
dsCloud.to2dArray()[:, :3], cubeSize / 2.0)
def callback(cloud_msg):
global callbackOnceOnly
if not callbackOnceOnly:
return
callbackOnceOnly = False
# convert from point cloud message to cloudprocpy.CloudXYZ
points_gen = read_points(cloud_msg, skip_nans=False)
points = []
for pt in points_gen:
pt = list(pt)
pt.append(1)
points.append(pt)
cloud = cloudprocpy.CloudXYZ()
xyz1 = np.array(points)
# cloud has to be organized for generateBodies to work
# hardcoded height and width
xyz1 = np.reshape(xyz1, (480, 640, 4))
cloud.from3dArray(xyz1)
global cloud_handle
global names
global id
# add new bodies and cloud first
names2 = generateBodies(cloud, id)
#cloud_handle2 = plotCloud(cloud)
# remove previous bodies and cloud
for name in names:
env.Remove(env.GetKinBody(name))
del cloud_handle
names = names2
#cloud_handle = cloud_handle2
id = (id + 1) % 2