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)
def downsample_cloud(cloud_xyz):
return clouds.downsample(cloud_xyz, DS_SIZE)