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)