def get_mask(rgb, depth, T_w_k):
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
return good_mask
def get_PC (self):
r, d = self.grabber.getRGBD()
x = clouds.depth_to_xyz(d, berkeley_pr2.f)
if Globals.pr2 is not None:
Globals.pr2.update_rave()
tfm = berkeley_pr2.get_kinect_transform(Globals.robot)
x = x.dot(tfm[:3,:3].T) + tfm[:3,3][None,None,:]
return ru.xyzrgb2pc(x, r, 'base_footprint')
def label_single_demo_points(seg_group, name):
if 'old_cloud_xyz' not in seg_group:
seg_group['old_cloud_xyz'] = seg_group['cloud_xyz'][:]
print name
rgb = seg_group['rgb'][:]
old_cloud_xyz = seg_group['old_cloud_xyz'][:]
depth = seg_group['depth'][:]
T_w_k = seg_group['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, :]
old_cloud_kd = KDTree(old_cloud_xyz)
image = np.asarray(rgb)
windowName = name
ctr = 0
finished = False
labeled_points = []
cv2.namedWindow(windowName)
cv2.setMouseCallback(windowName, mark2, (image, labeled_points))
while(1):
cv2.imshow(windowName,image)
k = cv2.waitKey(20) & 0xFF
if k == ord('r'):
return "retry"
elif k == ord(' '):
break
elif k == ord('p'):
return "restart"
elif k == 27:
return "quit"
labeled_points = np.asarray(labeled_points)
if 'labeled_points' in seg_group:
del seg_group['labeled_points']
seg_group.create_dataset("labeled_points",shape=(len(labeled_points),3),data=labeled_points)
# resample the line
labeled_xyz = []
for i in range(labeled_points.shape[0]):
pt_i = labeled_points[i, :2]
x, y, z = xyz_k[pt_i[0], pt_i[1], :].dot(T_w_k[:3, :3].T) + T_w_k[:3, 3]
if depth[pt_i[0], pt_i[1]] > 0 and z > .7:
labeled_xyz.append([x, y, z])
labeled_xyz = np.asarray(labeled_xyz)
cloud_xyz = np.zeros((resample_size, 3))
rope_len = line_length(labeled_xyz)
for i, t in enumerate(np.linspace(0, rope_len, resample_size)):
cloud_xyz[i, :] = point_at(labeled_xyz, t)
plt.scatter(old_cloud_xyz[:, 0], old_cloud_xyz[:, 1], color='r')
plt.scatter(cloud_xyz[:, 0], cloud_xyz[:, 1], color='b')
plt.show(block=False)
k = cv2.waitKey(0) & 0xFF
plt.close()
if k == ord('r'):
return "retry"
if 'cloud_xyz' in seg_group:
del seg_group['cloud_xyz']
seg_group['cloud_xyz'] = cloud_xyz
def find_keypoint_transform_execution (kp, grabber, tfm, normal=False):
global server, app, found, done
found = False
done = False
app = None
if rospy.get_name() == '/unnamed':
rospy.init_node("find_keypoint_%s"%kp, anonymous=True, disable_signals=True)
pcpub = rospy.Publisher('keypoint_pcd', PointCloud2)
server = InteractiveMarkerServer("find_keypoint_%s"%kp)
menu_handler.insert("Done?", callback=doneCallback)
menu_handler.insert("Re-try?", callback=redoCallback)
make6DofMarker( False, normal )
#make6DofMarker( True )
server.applyChanges()
print "Open an RViz Window!"
#if yes_or_no("Do you want to open your own rviz window? Might be a better idea."):
outer_RVIZ = True
# else:
# outer_RVIZ = False
# app = myviz.QApplication( [""] )
# viz = myviz.MyViz()
# viz.resize( 1000, 1000 )
# viz.show()
p = rospy.Rate(10)
while not rospy.is_shutdown() and not done:
rgb, dep = grabber.getRGBD()
xyz = clouds.depth_to_xyz(dep, berkeley_pr2.f)
xyz = xyz.dot(tfm[:3,:3].T) + tfm[:3,3][None,None,:]
pcmsg = ru.xyzrgb2pc(xyz, rgb, 'base_footprint')
pcpub.publish(pcmsg)
if not outer_RVIZ:
app.processEvents()
p.sleep()
# Unhappy with data
if not found:
return None
marker = server.get("pose_marker")
pose = marker.pose
print "Pose of the marker for %s"%kp
print pose
server.erase("pose_marker")
#rospy.signal_shutdown('Finished finding transform')
return conversions.pose_to_hmat(pose)
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 # XXXX 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]
cloud = cloudprocpy.CloudXYZ()
good_xyz1 = np.zeros((len(good_xyz), 4), 'float32')
good_xyz1[:,:3] = good_xyz
cloud.from2dArray(good_xyz1)
cloud_ds = cloudprocpy.downsampleCloud(cloud, .01)
return cloud_ds.to2dArray()[:,:3]
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 # XXXX 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]
cloud = cloudprocpy.CloudXYZ()
good_xyz1 = np.zeros((len(good_xyz), 4), 'float32')
good_xyz1[:, :3] = good_xyz
cloud.from2dArray(good_xyz1)
cloud_ds = cloudprocpy.downsampleCloud(cloud, .01)
return cloud_ds.to2dArray()[:, :3]
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]
h_mask = (h<15) | (h>145)
s_mask = (s > 30 )
v_mask = (v > 100)
red_mask = h_mask & s_mask & v_mask
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]
height_mask = xyz_w[:,:,2] > .7 # TODO pass in parameter
good_mask = red_mask & height_mask & valid_mask
good_mask = skim.remove_small_objects(good_mask,min_size=64)
if DEBUG_PLOTS:
cv2.imshow("z0",z0/z0.max())
cv2.imshow("z",z/z.max())
cv2.imshow("hue", h_mask.astype('uint8')*255)
cv2.imshow("sat", s_mask.astype('uint8')*255)
cv2.imshow("val", v_mask.astype('uint8')*255)
cv2.imshow("final",good_mask.astype('uint8')*255)
cv2.imshow("rgb", rgb)
cv2.waitKey()
good_xyz = xyz_w[good_mask]
return clouds.downsample(good_xyz, .025)
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, .025)
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, .025)
rave_traj = [
r2r.convert(row)
for row in asarray(seg_info["joint_states"]["position"])
]
robot.SetActiveDOFs(r2r.rave_inds)
robot.SetActiveDOFValues(rave_traj[0])
handles = []
T_w_k = berkeley_pr2.get_kinect_transform(robot)
o = T_w_k[:3, 3]
x = T_w_k[:3, 0]
y = T_w_k[:3, 1]
z = T_w_k[:3, 2]
handles.append(env.drawarrow(o, o + .3 * x, .005, (1, 0, 0, 1)))
handles.append(env.drawarrow(o, o + .3 * y, .005, (0, 1, 0, 1)))
handles.append(env.drawarrow(o, o + .3 * z, .005, (0, 0, 1, 1)))
XYZ_k = clouds.depth_to_xyz(np.asarray(seg_info["depth"]), berkeley_pr2.f)
Twk = asarray(seg_info["T_w_k"])
XYZ_w = XYZ_k.dot(Twk[:3, :3].T) + Twk[:3, 3][None, None, :]
RGB = np.asarray(seg_info["rgb"])
handles.append(
env.plot3(XYZ_w.reshape(-1, 3), 2,
RGB.reshape(-1, 3)[:, ::-1] / 255.))
animate_traj.animate_traj(rave_traj, robot, pause=not args.nopause)
print "DONE"
trajoptpy.GetViewer(env).Idle()
robot.SetActiveDOFs(r2r.rave_inds)
robot.SetActiveDOFValues(rave_traj[0])
handles = []
T_w_k = berkeley_pr2.get_kinect_transform(robot)
o = T_w_k[:3,3]
x = T_w_k[:3,0]
y = T_w_k[:3,1]
z = T_w_k[:3,2]
handles.append(env.drawarrow(o, o+.3*x, .005,(1,0,0,1)))
handles.append(env.drawarrow(o, o+.3*y, .005,(0,1,0,1)))
handles.append(env.drawarrow(o, o+.3*z, .005,(0,0,1,1)))
XYZ_k = clouds.depth_to_xyz(np.asarray(seg_info["depth"]), berkeley_pr2.f)
Twk = asarray(seg_info["T_w_k"])
XYZ_w = XYZ_k.dot(Twk[:3,:3].T) + Twk[:3,3][None,None,:]
RGB = np.asarray(seg_info["rgb"])
handles.append(env.plot3(XYZ_w.reshape(-1,3), 2, RGB.reshape(-1,3)[:,::-1]/255.))
animate_traj.animate_traj(rave_traj, robot, pause = not args.nopause)
print "DONE"
trajoptpy.GetViewer(env).Idle()
env = openravepy.Environment()
viewer = trajoptpy.GetViewer(env)
if args.ropefile is None:
demo_rope_handle = env.plot3(demo_rope, 10, (0,1,1,1))
perturbed_rope_handle = env.drawlinestrip(perturbed_rope, 10, (1,0,1,1))
else:
perturbed_rope_handle = env.plot3(perturbed_rope, 16, (0,0,0,1))
try:
grabber = cloudprocpy.CloudGrabber()
grabber.startRGBD()
while True:
rgb, depth = grabber.getRGBD()
XYZ_k = clouds.depth_to_xyz(depth, berkeley_pr2.f)
Twk = berkeley_pr2.get_kinect_transform(pr2.robot)
XYZ_w = XYZ_k.dot(Twk[:3,:3].T) + Twk[:3,3][None,None,:]
cloud_handle = env.plot3(XYZ_w.reshape(-1,3), 2, rgb.reshape(-1,3)[:,::-1]/255.)
viewer.Step()
except:
import traceback
traceback.print_exc()
finally:
grabber.stop()
def getPC ():
global pc
r, d = grabber.getRGBD()
x = clouds.depth_to_xyz(d, berkeley_pr2.f)
pc = ru.xyzrgb2pc(x, r, 'camera_link_p')
import numpy as np
import itertools
import cv2
from rapprentice import ros2rave
import sensorsimpy, trajoptpy, openravepy
from rapprentice import berkeley_pr2
import scipy.optimize as opt
from rapprentice import clouds
DEBUG_PLOT = True
npzfile = np.load(args.fname)
depth_images = npzfile["depth_images"]
valid_masks = [(d > 0) & ((clouds.depth_to_xyz(d, 525)**2).sum(axis=2) < 1.5**2) for d in depth_images]
depth_images = depth_images/1000.
joint_positions = npzfile["joint_positions"]
joint_names = npzfile["names"] if "names" in npzfile else npzfile["joint_names"]
env = openravepy.Environment()
env.StopSimulation()
env.Load("robots/pr2-beta-static.zae")
robot = env.GetRobots()[0]
viewer = trajoptpy.GetViewer(env)
viewer.Step()
sensor = sensorsimpy.CreateFakeKinect(env)
r2r = ros2rave.RosToRave(robot, joint_names)
import numpy as np
import itertools
import cv2
from rapprentice import ros2rave
import sensorsimpy, trajoptpy, openravepy
from rapprentice import berkeley_pr2
import scipy.optimize as opt
from rapprentice import clouds
DEBUG_PLOT = True
npzfile = np.load(args.fname)
depth_images = npzfile["depth_images"]
valid_masks = [
(d > 0) & ((clouds.depth_to_xyz(d, 525)**2).sum(axis=2) < 1.5**2)
for d in depth_images
]
depth_images = depth_images / 1000.
joint_positions = npzfile["joint_positions"]
joint_names = npzfile["names"] if "names" in npzfile else npzfile["joint_names"]
env = openravepy.Environment()
env.StopSimulation()
env.Load("robots/pr2-beta-static.zae")
robot = env.GetRobots()[0]
viewer = trajoptpy.GetViewer(env)
viewer.Step()
sensor = sensorsimpy.CreateFakeKinect(env)
