def __init__(self, full_cloud, id=None, full_color=None, downsample_size=0): """Inits SceneState Args: full_cloud: full (i.e. not downsampled) cloud id: unique id for this SceneState full_color: colors for the respective points of full_cloud. Should have the same dimensions as full_cloud downsample_size: if downsample_size is positive, the full cloud and color are downsampled to a voxel size of downsample_size, else they are not downsampled """ self.full_cloud = full_cloud self.full_color = full_color if downsample_size > 0: global clouds from lfd.rapprentice import clouds if full_color is not None: cloud_color = clouds.downsample(np.c_[full_cloud, full_color], downsample_size) self.cloud = cloud_color[:, :3] self.color = cloud_color[:, 3:] else: self.cloud = clouds.downsample(full_cloud, downsample_size) self.color = None else: self.cloud = full_cloud self.color = full_color if id is None: self.id = SceneState.get_unique_id() else: self.id = id
def __init__(self, full_cloud, id=None, full_color=None, downsample_size=0): """Inits SceneState Args: full_cloud: full (i.e. not downsampled) cloud id: unique id for this SceneState full_color: colors for the respective points of full_cloud. Should have the same dimensions as full_cloud downsample_size: if downsample_size is positive, the full cloud and color are downsampled to a voxel size of downsample_size, else they are not downsampled """ self.full_cloud = full_cloud self.full_color = full_color if downsample_size > 0: global clouds from lfd.rapprentice import clouds if full_color is not None: cloud_color = clouds.downsample(np.c_[full_cloud, full_color], downsample_size) self.cloud = cloud_color[:,:3] self.color = cloud_color[:,3:] else: self.cloud = clouds.downsample(full_cloud, downsample_size) self.color = None else: self.cloud = full_cloud self.color = full_color if id is None: self.id = SceneState.get_unique_id() else: self.id = id
def grabcut(rgb, depth, T_w_k): 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, :] valid_mask = depth > 0 import interactive_roi as ir xys = ir.get_polyline(rgb, "rgb") xy_corner1 = np.clip(np.array(xys).min(axis=0), [0, 0], [639, 479]) xy_corner2 = np.clip(np.array(xys).max(axis=0), [0, 0], [639, 479]) polymask = ir.mask_from_poly(xys) #cv2.imshow("mask",mask) xy_tl = np.array([xy_corner1, xy_corner2]).min(axis=0) xy_br = np.array([xy_corner1, xy_corner2]).max(axis=0) xl, yl = xy_tl w, h = xy_br - xy_tl mask = np.zeros((h, w), dtype='uint8') mask[polymask[yl:yl + h, xl:xl + w] > 0] = cv2.GC_PR_FGD print mask.shape #mask[h//4:3*h//4, w//4:3*w//4] = cv2.GC_PR_FGD tmp1 = np.zeros((1, 13 * 5)) tmp2 = np.zeros((1, 13 * 5)) cv2.grabCut(rgb[yl:yl + h, xl:xl + w, :], mask, (0, 0, 0, 0), tmp1, tmp2, 10, mode=cv2.GC_INIT_WITH_MASK) mask = mask % 2 #mask = ndi.binary_erosion(mask, utils_images.disk(args.erode)).astype('uint8') contours = cv2.findContours(mask, cv2.RETR_LIST, cv2.CHAIN_APPROX_NONE)[0] cv2.drawContours(rgb[yl:yl + h, xl:xl + w, :], contours, -1, (0, 255, 0), thickness=2) cv2.imshow('rgb', rgb) print "press enter to continue" cv2.waitKey() zsel = xyz_w[yl:yl + h, xl:xl + w, 2] mask = (mask % 2 == 1) & np.isfinite(zsel) # & (zsel - table_height > -1) mask &= valid_mask[yl:yl + h, xl:xl + w] xyz_sel = xyz_w[yl:yl + h, xl:xl + w, :][mask.astype('bool')] return clouds.downsample(xyz_sel, .01)
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 # TODO 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] return clouds.downsample(good_xyz, .01)
def grabcut(rgb, depth, T_w_k): 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,:] valid_mask = depth > 0 import interactive_roi as ir xys = ir.get_polyline(rgb, "rgb") xy_corner1 = np.clip(np.array(xys).min(axis=0), [0,0], [639,479]) xy_corner2 = np.clip(np.array(xys).max(axis=0), [0,0], [639,479]) polymask = ir.mask_from_poly(xys) #cv2.imshow("mask",mask) xy_tl = np.array([xy_corner1, xy_corner2]).min(axis=0) xy_br = np.array([xy_corner1, xy_corner2]).max(axis=0) xl, yl = xy_tl w, h = xy_br - xy_tl mask = np.zeros((h,w),dtype='uint8') mask[polymask[yl:yl+h, xl:xl+w] > 0] = cv2.GC_PR_FGD print mask.shape #mask[h//4:3*h//4, w//4:3*w//4] = cv2.GC_PR_FGD tmp1 = np.zeros((1, 13 * 5)) tmp2 = np.zeros((1, 13 * 5)) cv2.grabCut(rgb[yl:yl+h, xl:xl+w, :],mask,(0,0,0,0),tmp1, tmp2,10,mode=cv2.GC_INIT_WITH_MASK) mask = mask % 2 #mask = ndi.binary_erosion(mask, utils_images.disk(args.erode)).astype('uint8') contours = cv2.findContours(mask,cv2.RETR_LIST,cv2.CHAIN_APPROX_NONE)[0] cv2.drawContours(rgb[yl:yl+h, xl:xl+w, :],contours,-1,(0,255,0),thickness=2) cv2.imshow('rgb', rgb) print "press enter to continue" cv2.waitKey() zsel = xyz_w[yl:yl+h, xl:xl+w, 2] mask = (mask%2==1) & np.isfinite(zsel)# & (zsel - table_height > -1) mask &= valid_mask[yl:yl+h, xl:xl+w] xyz_sel = xyz_w[yl:yl+h, xl:xl+w,:][mask.astype('bool')] return clouds.downsample(xyz_sel, .01)
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 # TODO 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] return clouds.downsample(good_xyz, .01)
def downsample_cloud(cloud_xyz): return clouds.downsample(cloud_xyz, DS_SIZE)