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)