def removeBorders(self):
print "what's the radius of the structuring element? (pixels)"
keycode = cv2.waitKey(0) % 256
try:
radius = int(chr(keycode))
except ValueError:
print "error: must be an number (0-9)"
return
sel = utils_images.disk(radius)
for iLabel in xrange(1,self.maxLabel+1):
self.label[(self.label == iLabel) &
ndi.binary_dilation((self.label != iLabel) & (self.label > 0), sel)] = 0
self.updateDisplay()
from jds_image_proc import pcl_utils, pcd_io, utils_images
import cPickle
from mlabraw_jds_image_processing import remove_holes
import cv2
import numpy as np
import scipy.ndimage as ndi
xyz, bgr = pcd_io.load_xyzrgb("/home/joschu/comm/pr2_knot/kinect/data000000000100.pcd")
bgr = bgr.copy()
mask = cv2.imread("/home/joschu/comm/pr2_knot/once/roi_mask.png")[:,:,0].copy()
with open("/home/joschu/comm/pr2_knot/human_labels/classifier.pkl","r") as fh: cls = cPickle.load(fh)
labels = cls.predict(bgr, mask=mask)
x,y,z = xyz[:,:,0], xyz[:,:,1], xyz[:,:,2]
depth = np.sqrt(x**2+y**2+z**2)
depth[np.isnan(depth)] = np.inf
bad = np.zeros(depth.shape,bool)
bad[1:] |= np.abs(depth[1:] - depth[:-1]) > .01
bad[:,1:] |= np.abs(depth[:,1:] - depth[:,:-1]) > .1
bad = ndi.binary_dilation(bad, utils_images.disk(2))
cv2.imshow('bad', bad.astype('uint8')*100)
cv2.imshow('labels',labels*100*~bad)
labels1 = labels*~bad
cv2.imshow('cleaned labels',labels1*100*~bad)
from jds_image_proc import pcl_utils, pcd_io, utils_images
import cPickle
from mlabraw_jds_image_processing import remove_holes
import cv2
import numpy as np
import scipy.ndimage as ndi
xyz, bgr = pcd_io.load_xyzrgb(
"/home/joschu/comm/pr2_knot/kinect/data000000000100.pcd")
bgr = bgr.copy()
mask = cv2.imread("/home/joschu/comm/pr2_knot/once/roi_mask.png")[:, :,
0].copy()
with open("/home/joschu/comm/pr2_knot/human_labels/classifier.pkl", "r") as fh:
cls = cPickle.load(fh)
labels = cls.predict(bgr, mask=mask)
x, y, z = xyz[:, :, 0], xyz[:, :, 1], xyz[:, :, 2]
depth = np.sqrt(x**2 + y**2 + z**2)
depth[np.isnan(depth)] = np.inf
bad = np.zeros(depth.shape, bool)
bad[1:] |= np.abs(depth[1:] - depth[:-1]) > .01
bad[:, 1:] |= np.abs(depth[:, 1:] - depth[:, :-1]) > .1
bad = ndi.binary_dilation(bad, utils_images.disk(2))
cv2.imshow('bad', bad.astype('uint8') * 100)
cv2.imshow('labels', labels * 100 * ~bad)
labels1 = labels * ~bad
cv2.imshow('cleaned labels', labels1 * 100 * ~bad)
def callback(req):
xyz, rgb = pc2xyzrgb(req.cloud_in)
rgb = rgb.copy()
gc = GetClick()
cv2.setMouseCallback("rgb", gc.callback)
while not gc.done:
cv2.imshow("rgb", rgb)
cv2.waitKey(10)
gm = GetMouse()
xy_corner1 = np.array(gc.xy)
cv2.setMouseCallback("rgb", gm.callback)
while not gm.done:
img = rgb.copy()
if gm.xy is not None:
xy_corner2 = np.array(gm.xy)
cv2.rectangle(img, tuple(xy_corner1), tuple(xy_corner2), (0,255, 0))
cv2.imshow("rgb",img)
cv2.waitKey(10)
xy_tl = np.array([xy_corner1, xy_corner2]).min(axis=0)
xy_br = np.array([xy_corner1, xy_corner2]).max(axis=0)
#mask = np.zeros(xyz.shape[:2],dtype='uint8')
#rectangle = gm.xy + tuple(2*(center - np.r_[gm.xy]))
#tmp1 = np.zeros((1, 13 * 5))
#tmp2 = np.zeros((1, 13 * 5))
#result = cv2.grabCut(rgb,mask,rectangle,tmp1, tmp2,10,mode=cv2.GC_INIT_WITH_RECT)
#from cv2 import cv
#mask[mask == cv.GC_BGD] = 0
#mask[mask == cv.GC_PR_BGD] = 63
#mask[mask == cv.GC_FGD] = 255
#mask[mask == cv.GC_PR_FGD] = 192
#cv2.imshow('mask',mask)
#cv2.waitKey(10)
#table_height = get_table_height(xyz)
if args.grabcut:
rospy.loginfo("using grabcut")
xl, yl = xy_tl
w, h = xy_br - xy_tl
mask = np.zeros((h,w),dtype='uint8')
mask.fill(cv2.GC_PR_BGD)
if args.init == "height":
initial_height_thresh = stats.scoreatpercentile(xyz[yl:yl+h, xl:xl+w,2].flatten(), 50)
mask[xyz[yl:yl+h, xl:xl+w,2] > initial_height_thresh] = cv2.GC_PR_FGD
elif args.init == "middle":
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)
else:
rospy.loginfo("using table height")
table_height = rospy.get_param("table_height")
xl, yl = xy_tl
w, h = xy_br - xy_tl
mask = np.zeros((h,w),dtype='uint8')
mask[np.isfinite(xyz[yl:yl+h, xl:xl+w,2]) &
(xyz[yl:yl+h, xl:xl+w,2] > table_height+.02)] = 1
mask = mask % 2
if (args.erode > 0):
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)
cv2.waitKey(10)
zsel = xyz[yl:yl+h, xl:xl+w, 2]
mask = (mask%2==1) & np.isfinite(zsel)# & (zsel - table_height > -1)
resp = ProcessCloudResponse()
xyz_sel = xyz[yl:yl+h, xl:xl+w,:][mask.astype('bool')]
rgb_sel = rgb[yl:yl+h, xl:xl+w,:][mask.astype('bool')]
resp.cloud_out = xyzrgb2pc(xyz_sel, rgb_sel, req.cloud_in.header.frame_id)
if (args.plotting):
pose_array = conversions.array_to_pose_array(xyz_sel, req.cloud_in.header.frame_id)
Globals.handles = [Globals.rviz.draw_curve(pose_array, rgba = (1,1,0,1), type=Marker.CUBE_LIST, width=.001, ns="segmentation")]
return resp
