def visualize_pointcloud (camera_frame="camera_depth_optical_frame", device_id="#1"):
"""
Visualize point clouds from openni grabber through ROS.
"""
if rospy.get_name() == '/unnamed':
rospy.init_node("visualize_pointcloud")
pc_pub = rospy.Publisher("camera_points", PointCloud2)
sleeper = rospy.Rate(30)
grabber = cpr.CloudGrabber(device_id)
grabber.startRGBD()
print "Streaming now from frame %s: Pointclouds only."%camera_frame
while True:
try:
r, d = grabber.getRGBD()
pc = ru.xyzrgb2pc(clouds.depth_to_xyz(d, asus_xtion_pro_f), r, camera_frame)
pc_pub.publish(pc)
sleeper.sleep()
except KeyboardInterrupt:
print "Keyboard interrupt. Exiting."
break
def get_click_points (rgb, depth):
"""
Get clicked points in an image.
Add functions if you want to calibrate based on color or something.
"""
clicks = []
def mouse_click(event, x, y, flags, params):
if event == cv.CV_EVENT_LBUTTONDOWN:
clicks.append([x, y])
xyz = clouds.depth_to_xyz(depth, asus_xtion_pro_f)
cv2.imshow("Points", rgb)
cv.SetMouseCallback("Points", mouse_click, 0)
print "Click the points keeping the order in mind. Press enter to continue."
cv2.waitKey()
cv2.destroyWindow("Points")
clickPoints = []
for (x, y) in clicks:
clickPoints.append(np.copy(xyz[y][x]))
return clickPoints
def publish_transform_markers(grabber, marker, T, from_frame, to_frame, rate=100):
from visualization_msgs.msg import Marker
from sensor_msgs.msg import PointCloud2
print colorize("Transform : ", "yellow", True)
print T
marker_frame = "ar_frame"
tf_pub = tf.TransformBroadcaster()
pc_pub = rospy.Publisher("camera_points", PointCloud2)
trans = T[0:3,3]
rot = tf.transformations.quaternion_from_matrix(T)
sleeper = rospy.Rate(10)
while True:
try:
r, d = grabber.getRGBD()
ar_tfm = get_ar_transform_id (d, r, marker)
pc = ru.xyzrgb2pc(clouds.depth_to_xyz(d, asus_xtion_pro_f), r, from_frame)
pc_pub.publish(pc)
tf_pub.sendTransform(trans, rot, rospy.Time.now(), to_frame, from_frame)
try:
trans_m, rot_m = conversions.hmat_to_trans_rot(ar_tfm)
tf_pub.sendTransform(trans_m, rot_m, rospy.Time.now(), marker_frame, from_frame)
except:
pass
sleeper.sleep()
except KeyboardInterrupt:
break
def extract_rope_tracking(rgb, depth, T_w_k):
red_mask = [lambda(x): (x<25)|(x>100), lambda(x): x>80, lambda(x): x>100]
hsv = cv2.cvtColor(rgb, cv2.COLOR_BGR2HSV)
h = hsv[:,:,0]
s = hsv[:,:,1]
v = hsv[:,:,2]
h_mask = red_mask[0](h)
s_mask = red_mask[1](s)
v_mask = red_mask[2](v)
color_mask = h_mask & s_mask & v_mask
valid_mask = (depth > 0)
xyz_k = clouds.depth_to_xyz(depth, asus_xtion_pro_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' or distance from the camera
height_mask = (xyz_w[:,:,2] > 0) & (xyz_w[:,:,2] < 1.1)
good_mask = color_mask & valid_mask & height_mask
# return xyz_k instead of xyz_w ==> change to use xyz_w
good_xyz = xyz_w[good_mask]
good_rgb = rgb[good_mask]
colored_xyz = np.hstack((good_xyz, good_rgb))
return colored_xyz
def get_ar_marker_poses (rgb, depth, pc=None):
"""
In order to run this, ar_marker_service needs to be running.
"""
global getMarkers, req, ar_initialized
if not ar_initialized:
if rospy.get_name() == '/unnamed':
rospy.init_node('ar_tfm')
getMarkers = rospy.ServiceProxy("getMarkers", MarkerPositions)
req = MarkerPositionsRequest()
ar_initialized = True
if pc is None:
xyz = clouds.depth_to_xyz(depth, asus_xtion_pro_f)
pc = ru.xyzrgb2pc(xyz, rgb, '/camera_frame')
req.pc = pc
marker_tfm = {}
res = getMarkers(req)
for marker in res.markers.markers:
marker_tfm[marker.id] = conversions.pose_to_hmat(marker.pose.pose)
return marker_tfm
def run (self):
"""
Publishes the transforms stored.
"""
while True:
if self.publish_pc and self.cameras is not None:
data=self.cameras.get_RGBD()
for i,rgbd in data.items():
if rgbd['depth'] is None or rgbd['rgb'] is None:
print 'wut'
continue
camera_frame = 'camera%d_depth_optical_frame'%(i+1)
xyz = clouds.depth_to_xyz(rgbd['depth'], asus_xtion_pro_f)
pc = ru.xyzrgb2pc(xyz, rgbd['rgb'], camera_frame)
ar_cam = gmt.get_ar_marker_poses(None, None, pc=pc)
self.pc_pubs[i].publish(pc)
marker_frame = 'ar_marker%d_camera%d'
for marker in ar_cam:
trans, rot = conversions.hmat_to_trans_rot(ar_cam[marker])
self.tf_broadcaster.sendTransform(trans, rot,
rospy.Time.now(),
marker_frame%(marker,i+1),
camera_frame)
if self.ready:
for parent, child in self.transforms:
trans, rot = self.transforms[parent, child]
self.tf_broadcaster.sendTransform(trans, rot,
rospy.Time.now(),
child, parent)
time.sleep(1/self.rate)
def extract_cloud(depth, T_w_k):
xyz_k = clouds.depth_to_xyz(depth, asus_xtion_pro_f)
xyz_w = xyz_k.dot(T_w_k[:3,:3].T) + T_w_k[:3,3][None,None,:]
valid_mask = (depth > 0)
good_xyz = xyz_w[valid_mask]
return good_xyz
def get_pc(self, index=None):
if not index:
index = self.index
rgb = cv2.imread(self.rgbs_fnames[index])
depth = cv2.imread(self.depth_fnames[index], 2)
xyz = clouds.depth_to_xyz(depth, asus_xtion_pro_f)
pc = ru.xyzrgb2pc(xyz, rgb, frame_id=self.frame_id, use_time_now=False)
return pc
def get_ar_transform_id (depth, rgb, idm=None):
"""
In order to run this, ar_marker_service needs to be running.
"""
req.pc = ru.xyzrgb2pc(clouds.depth_to_xyz(depth, asus_xtion_pro_f), rgb, '/camera_link')
res = getMarkers(req)
marker_tfm = {marker.id:conversions.pose_to_hmat(marker.pose.pose) for marker in res.markers.markers}
if not idm: return marker_tfm
if idm not in marker_tfm: return None
return marker_tfm[idm]
def get_tfm_of_camera(ind1,ind2):
rgbd1_dir = '/home/sibi/sandbox/human_demos/hd_data/demos/sibi_demo/camera_#1'
rgbd2_dir = '/home/sibi/sandbox/human_demos/hd_data/demos/sibi_demo/camera_#2'
rgbs1fnames, depths1fnames, stamps1 = eu.get_rgbd_names_times(rgbd1_dir)
rgbs2fnames, depths2fnames, stamps2 = eu.get_rgbd_names_times(rgbd2_dir)
winname = 'abcd'
ar1, ar2 = None, None
rgb1 = cv2.imread(rgbs1fnames[ind1])
assert rgb1 is not None
depth1 = cv2.imread(depths1fnames[ind1],2)
assert depth1 is not None
cv2.imshow(winname,)
xyz1 = clouds.depth_to_xyz(depth1, asus_xtion_pro_f)
pc1 = ru.xyzrgb2pc(xyz1, rgb1, frame_id='', use_time_now=False)
ar_tfms1 = get_ar_marker_poses(pc1)
if ar_tfms1 and 1 in ar_tfms1:
blueprint("Got markers " + str(ar_tfms1.keys()) + " at time %f"%stamps1[ind1])
ar1 = ar_tfms1[1]
rgb2 = cv2.imread(rgbs2fnames[ind2])
assert rgb2 is not None
depth2 = cv2.imread(depths2fnames[ind2],2)
assert depth2 is not None
xyz2 = clouds.depth_to_xyz(depth2, asus_xtion_pro_f)
pc2 = ru.xyzrgb2pc(xyz2, rgb2, frame_id='', use_time_now=False)
ar_tfms2 = get_ar_marker_poses(pc2)
if ar_tfms2 and 1 in ar_tfms2:
blueprint("Got markers " + str(ar_tfms2.keys()) + " at time %f"%stamps1[ind2])
ar2 = ar_tfms2[1]
if ar1 is None or ar2 is None:
return None
return ar1.dot(nlg.inv(ar2))
def extract_color(rgb, depth, mask, T_w_k, xyz_mask=None, use_outlier_removal=False, outlier_thresh=2, outlier_k=20):
"""
extract red points and downsample
"""
hsv = cv2.cvtColor(rgb, cv2.COLOR_BGR2HSV)
h = hsv[:,:,0]
s = hsv[:,:,1]
v = hsv[:,:,2]
h_mask = mask[0](h)
s_mask = mask[1](s)
v_mask = mask[2](v)
color_mask = h_mask & s_mask & v_mask
valid_mask = (depth > 0)
xyz_k = clouds.depth_to_xyz(depth, asus_xtion_pro_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' or distance from the camera
height_mask = (xyz_w[:,:,2] > 0.6) & (xyz_w[:,:,2] < 1.1)
if xyz_mask: height_mask = height_mask & xyz_mask(xyz_w)
good_mask = color_mask & valid_mask & height_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]
if use_outlier_removal:
#good_xyz = clouds.remove_outliers(good_xyz, outlier_thresh, outlier_k)
#good_xyz = remove_outlier_connected_component(good_xyz)
good_xyz = clouds.cluster_filter(good_xyz, 0.03, int(0.4 * len(good_xyz)))
return good_xyz
def calibrate_cb (rgb1, depth1, rgb2, depth2):
"""
Find CB transform for each cam and then return a transform such that
Tfm * cam1 = cam2.
TODO: Find better method to get transform out of checkerboard.
"""
rtn1, corners1 = cbt.get_corners_rgb(rgb1, cb_rows, cb_cols)
xyz1 = clouds.depth_to_xyz(depth, asus_xtion_pro_f)
rtn2, corners2 = cbt.get_corners_rgb(rgb2, cb_rows, cb_cols)
xyz2 = clouds.depth_to_xyz(depth, asus_xtion_pro_f)
if rtn1 != rtn2:
print "Warning: One of the cameras do not see all the checkerboard points."
elif rtn1 == 0:
print "Warning: Not all the checkerboard points are seen by the cameras."
if len(corners1) != len(corners2):
print "Warning: Different number of points found by different cameras -- transform will be off."
tfm1 = cbt.get_svd_tfm_from_points(cbt.get_xyz_from_corners(corners1, xyz1))
tfm2 = cbt.get_svd_tfm_from_points(cbt.get_xyz_from_corners(corners2, xyz2))
return np.linalg.inv(tfm1).dot(tfm2)
def extract_hitch(rgb, depth, T_w_k, dir=None, radius=0.016, length =0.215, height_range=[0.70,0.80]):
"""
template match to find the hitch in the picture,
get the xyz at those points using the depth image,
extend the points down to aid in tps.
"""
template_fname = osp.join(hd_data_dir, 'hitch_template.jpg')
template = cv2.imread(template_fname)
h,w,_ = template.shape
res = cv2.matchTemplate(rgb, template, cv2.TM_CCORR_NORMED)
_, _, _, max_loc = cv2.minMaxLoc(res)
top_left = max_loc
bottom_right = (top_left[0]+w, top_left[1]+h)
if DEBUG_PLOTS:
cv2.rectangle(rgb,top_left, bottom_right, (255,0,0), thickness=2)
cv2.imshow("hitch detect", rgb)
cv2.waitKey()
# now get all points in a window around the hitch loc:
xyz_k = clouds.depth_to_xyz(depth, asus_xtion_pro_f)
xyz_w = xyz_k.dot(T_w_k[:3,:3].T) + T_w_k[:3,3][None,None,:]
hitch_pts = xyz_w[top_left[1]:bottom_right[1], top_left[0]:bottom_right[0],:]
height_mask = (hitch_pts[:,:,2] > height_range[0]) & (hitch_pts[:,:,2] < height_range[1])
hitch_pts = hitch_pts[height_mask]
## add pts along the rod:
center_xyz = np.median(hitch_pts, axis=0)
ang = np.linspace(0, 2*np.pi, 30)
circ_pts = radius*np.c_[np.cos(ang), np.sin(ang)] + center_xyz[None,:2]
circ_zs = np.linspace(center_xyz[2], length+center_xyz[2], 30)
rod_pts = np.empty((0,3))
for z in circ_zs:
rod_pts = np.r_[rod_pts, np.c_[circ_pts, z*np.ones((len(circ_pts),1))] ]
all_pts = np.r_[hitch_pts, rod_pts]
if dir is None:
return all_pts, center_xyz
else:
axis = np.cross([0,0,1], dir)
angle = np.arccos(np.dot([0,0,1], dir))
tfm = matrixFromAxisAngle(axis, angle)
tfm[:3,3] = - tfm[:3,:3].dot(center_xyz) + center_xyz
return np.asarray([tfm[:3,:3].dot(point) + tfm[:3,3] for point in all_pts]), center_xyz
def checkerboard_loop ():
rospy.init_node("checkerboard_tracker")
grabber = cpr.CloudGrabber()
grabber.startRGBD()
pc_pub = rospy.Publisher("cb_cloud", PointCloud2)
pose_pub = rospy.Publisher("cb_pose", PoseStamped)
rate = rospy.Rate(10)
while True:
try:
rgb, depth = grabber.getRGBD()
xyz = clouds.depth_to_xyz(depth, asus_xtion_pro_f)
rtn, corners = get_corners_rgb (rgb)
points = get_xyz_from_corners(corners, xyz)
print "Shape of points: ", points.shape
if points.any():
try:
tfm = get_svd_tfm_from_points(points)
except:
continue
print tfm
pc = ros_utils.xyzrgb2pc(xyz, rgb, "/map")
pose = PoseStamped()
pose.pose = conversions.hmat_to_pose(tfm)
pc.header.frame_id = pose.header.frame_id = "/map"
pose.header.stamp = pose.header.stamp = rospy.Time.now()
pose_pub.publish(pose)
pc_pub.publish(pc)
rate.sleep()
except KeyboardInterrupt:
break
def grabcut(rgb, depth, T_w_k):
xyz_k = clouds.depth_to_xyz(depth, asus_xtion_pro_f)
xyz_w = xyz_k.dot(T_w_k[:3,:3].T) + T_w_k[:3,3][None,None,:]
valid_mask = depth > 0
from hd_rapprentice 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 visualize_ar ():
"""
Visualize point clouds from openni grabber and AR marker transforms through ROS.
"""
global getMarkers
if rospy.get_name() == '/unnamed':
rospy.init_node("visualize_ar")
camera_frame = "camera_depth_optical_frame"
tf_pub = tf.TransformBroadcaster()
pc_pub = rospy.Publisher("camera_points", PointCloud2)
sleeper = rospy.Rate(30)
getMarkers = rospy.ServiceProxy("getMarkers", MarkerPositions)
grabber= cpr.CloudGrabber()
grabber.startRGBD()
print "Streaming now."
while True:
try:
r, d = grabber.getRGBD()
ar_tfms = get_ar_transform_id (d, r)
pc = ru.xyzrgb2pc(clouds.depth_to_xyz(d, asus_xtion_pro_f), r, camera_frame)
pc_pub.publish(pc)
for i in ar_tfms:
try:
trans, rot = conversions.hmat_to_trans_rot(ar_tfms[i])
tf_pub.sendTransform(trans, rot, rospy.Time.now(), "ar_marker_%d"%i, camera_frame)
except:
print "Warning: Problem with AR transform."
pass
sleeper.sleep()
except KeyboardInterrupt:
print "Keyboard interrupt. Exiting."
break
def get_ar_marker_poses (rgb, depth):
"""
In order to run this, ar_marker_service needs to be running.
"""
if rospy.get_name() == '/unnamed':
rospy.init_node('keypoints')
getMarkers = rospy.ServiceProxy("getMarkers", MarkerPositions)
#xyz = svi.transform_pointclouds(depth, tfm)
xyz = clouds.depth_to_xyz(depth, asus_xtion_pro_f)
pc = ru.xyzrgb2pc(xyz, rgb, '/base_footprint')
req = MarkerPositionsRequest()
req.pc = pc
marker_tfm = {}
res = getMarkers(req)
for marker in res.markers.markers:
marker_tfm[marker.id] = conversions.pose_to_hmat(marker.pose.pose)
#print "Marker ids found: ", marker_tfm.keys()
return marker_tfm
def get_markers_kinect():
subprocess.call("killall XnSensorServer", shell=True)
grabber = cloudprocpy.CloudGrabber()
grabber.startRGBD()
rgb, depth = grabber.getRGBD()
clicks = []
def mouse_click(event, x, y, flags, params):
if event == cv.CV_EVENT_LBUTTONDOWN:
clicks.append([x, y])
# rewrite this
def find_nearest(xy, indicators):
nearest = []
for (x, y) in xy:
min_dist = 10000000
closest_point = [0, 0]
for j in xrange(len(indicators)):
for i in xrange(len(indicators[0])):
dist = (x - i) ** 2 + (y - j) ** 2
if indicators[j][i] and dist < min_dist:
#print 'satisfied'
min_dist = dist
closest_point = [i, j]
nearest.append(closest_point)
return nearest
def find_avg(points, clicks, cloud):
avgPoints = []
rad = 0.02
r = 5
for (x,y), point in zip(clicks,points):
avg = np.array([0,0,0])
num = 0.0
for i in range(-r,r+1):
for j in range(-r,r+1):
cloudPt = cloud[y+i, x+j,:];
if not np.isnan(cloudPt).any() and np.linalg.norm(point - cloudPt) < rad:
avg = avg + cloudPt
num += 1.0
if num == 0.0:
print "not found"
avgPoints.append(point)
else:
print "found"
avgPoints.append(avg / num)
return avgPoints
def find_nearest_radius(points, clicks, cloud):
markerPoints = []
rad = 0.01
r = 5
for (x,y), point in zip(clicks,points):
checkPoints = cloud[max(0,y-r):min(cloud.shape[0]-1,y+r), max(0,x-r):min(cloud.shape[1]-1, x+r),:]
np.putmask(checkPoints, np.isnan(checkPoints), np.infty)
dist = checkPoints - point[None,None,:]
dist = np.sum((dist*dist), axis=2)
yi,xi = np.unravel_index(np.argmin(dist), dist.shape)
point = checkPoints[yi,xi]
if np.isnan(point).any() or np.min(dist) > rad:
print "No point found for clicked point: ", point
else:
markerPoints.append(point)
return markerPoints
hsv = cv2.cvtColor(rgb, cv2.COLOR_BGR2HSV)
h = hsv[:,:,0]
s = hsv[:,:,1]
v = hsv[:,:,2]
red_mask = (v>150) & ((h<10) | (h>150))# & (v > 200)# & (s > 100)
#valid = depth*(depth > 0)
xyz_k = clouds.depth_to_xyz(depth, asus_xtion_pro_f)
#cv2.imshow("red",red_mask.astype('uint8')*255)
cv2.imshow("red", red_mask.astype('uint8')*255)
cv2.imshow("rgb", rgb)
cv.SetMouseCallback("rgb", mouse_click, 0)
print "press enter to continue"
cv2.waitKey()
clickPoints = []
#get click points
for (x, y) in clicks:
clickPoints.append(np.copy(xyz_k[y][x]))
# import IPython
# IPython.embed()
red_mask3d = np.tile(np.atleast_3d(red_mask), [1,1,3])
np.putmask (xyz_k, red_mask3d != 1, np.NAN)
markerPoints = find_nearest_radius(clickPoints, clicks, xyz_k)
return markerPoints
def save_observations_rgbd(demo_type, demo_name, save_file=None):
"""
Extract data from all sensors and store into demo.data file.
"""
global setCalib
# Temp file to show that data is already being extracted
demo_dir = osp.join(demo_files_dir, demo_type, demo_name)
with open(osp.join(demo_dir, demo_names.extract_data_temp),'w') as fh: fh.write('Extracting...')
calib_file_path = osp.join(demo_dir, demo_names.calib_name)
bag_file = osp.join(demo_dir, demo_names.bag_name)
with open(osp.join(demo_dir, demo_names.camera_types_name)) as fh:
camera_types = yaml.load(fh)
with open(osp.join(demo_dir, demo_names.camera_models_name)) as fh:
camera_models = yaml.load(fh)
num_cameras = len(camera_types)
video_dirs = {}
for i in range(1, num_cameras + 1):
video_dirs[i] = osp.join(demo_dir, demo_names.video_dir%i)
c_frames = {}
for i in range(1, num_cameras + 1):
c_frames[i]= 'camera%i_link'%(i)
hydra_frame = 'hydra_base'
tfm_c1 = {i:None for i in range (1,num_cameras+1)}
tfm_c1[1] = np.eye(4)
tfm_c1_h = None
with open(calib_file_path,'r') as fh: calib_data = cPickle.load(fh)
bag = rosbag.Bag(bag_file)
for tfm in calib_data['transforms']:
if tfm['parent'] == c_frames[1] or tfm['parent'] == '/' + c_frames[1]:
if tfm['child'] == hydra_frame or tfm['child'] == '/' + hydra_frame:
tfm_c1_h = nlg.inv(tfm_link_rof).dot(tfm['tfm'])
else:
for i in range(2, num_cameras+1):
if tfm['child'] == c_frames[i] or tfm['child'] == '/' + c_frames[i]:
tfm_c1[i] = nlg.inv(tfm_link_rof).dot(tfm['tfm']).dot(tfm_link_rof)
if tfm_c1_h is None or not all([tfm_c1[s] != None for s in tfm_c1]):
redprint("Calibration does not have required transforms")
return
if not calib_data.get('grippers'):
redprint("Gripper not found.")
return
grippers = {}
data = {}
data['T_cam2hbase'] = tfm_c1_h
for lr,gdata in calib_data['grippers'].items():
gr = gripper_lite.GripperLite(lr, gdata['ar'], trans_marker_tooltip=gripper_trans_marker_tooltip[lr])
gr.reset_gripper(lr, gdata['tfms'], gdata['ar'], gdata['hydra'])
grippers[lr] = gr
data[lr] ={'hydra':[],
'pot_angles':[],
'T_tt2hy': gr.get_rel_transform('tool_tip', gr.hydra_marker)}
## place-holder for AR marker transforms:
for i in xrange(num_cameras):
data[lr]['camera%d'%(i+1)] = []
winname = 'cam_image'
cam_counts = []
for i in range(1,num_cameras+1):
if verbose:
yellowprint('Camera%i'%i)
if camera_types[i] == "rgbd":
rgb_fnames, depth_fnames, stamps = eu.get_rgbd_names_times(video_dirs[i])
else:
rgb_fnames, stamps = eu.get_rgbd_names_times(video_dirs[i], depth = False)
cam_count = len(stamps)
cam_counts.append(cam_count)
if camera_types[i] == "rgbd":
for ind in rgb_fnames:
rgb = cv2.imread(rgb_fnames[ind])
if displayImages:
cv2.imshow(winname, rgb)
cv2.waitKey(1)
assert rgb is not None
depth = cv2.imread(depth_fnames[ind],2)
assert depth is not None
xyz = clouds.depth_to_xyz(depth, asus_xtion_pro_f)
pc = ru.xyzrgb2pc(xyz, rgb, frame_id='', use_time_now=False)
ar_tfms = get_ar_marker_poses(pc,track=True)
if ar_tfms:
if verbose:
blueprint("Got markers " + str(ar_tfms.keys()) + " at time %f"%stamps[ind])
for lr,gr in grippers.items():
ar = gr.get_ar_marker()
if ar in ar_tfms:
tt_tfm = gr.get_tooltip_transform(ar, np.asarray(ar_tfms[ar]))
data[lr]['camera%i'%i].append((tfm_c1[i].dot(tt_tfm),stamps[ind]))
else:
# if setCalib is None:
# setCalib = rospy.ServiceProxy("setCalibInfo", SetCalibInfo)
# reqCalib.camera_model = camera_models[i]
# setCalib(reqCalib)
#
# if verbose:
# yellowprint("Changed camera calibration parameters to model %s"%camera_models[i])
for ind in rgb_fnames:
rgb = cv.LoadImage(rgb_fnames[ind])
if displayImages:
cv.ShowImage(winname, rgb)
cv.WaitKey(1)
assert rgb is not None
ar_tfms = get_ar_marker_poses(rgb,use_pc_service=False, model = camera_models[i], track=True)
if ar_tfms:
if verbose:
blueprint("Got markers " + str(ar_tfms.keys()) + " at time %f"%stamps[ind])
for lr,gr in grippers.items():
ar = gr.get_ar_marker()
if ar in ar_tfms:
tt_tfm = gr.get_tooltip_transform(ar, np.asarray(ar_tfms[ar]))
data[lr]['camera%i'%i].append((tfm_c1[i].dot(tt_tfm),stamps[ind]))
found_hydra_data = False
found_pot_data = False
# If hydra_only.data file already exists, don't do extra work.
hydra_data_file = osp.join(demo_dir, demo_names.hydra_data_name)
if osp.isfile(hydra_data_file):
yellowprint ("%s already exists for %s. Getting hydra data from this info."%(demo_names.hydra_data_name,demo_name))
with open(hydra_data_file,'r') as fh: hdata = cPickle.load(fh)
for lr in hdata:
if lr in 'lr':
if hdata[lr].get('hydra'):
data[lr]['hydra'] = hdata[lr]['hydra']
found_hydra_data = True
if hdata[lr].get('pot_angles'):
data[lr]['pot_angles'] = hdata[lr]['pot_angles']
found_pot_data = True
if not found_hydra_data:
if verbose:
yellowprint('Hydra')
lr_long = {'l':'left','r':'right'}
for (_, msg, _) in bag.read_messages(topics=['/tf']):
hyd_tfm = {}
found = ''
for tfm in msg.transforms:
if found in ['lr','rl']:
break
for lr in grippers:
if lr in found:
continue
elif tfm.header.frame_id == '/' + hydra_frame and tfm.child_frame_id == '/hydra_'+lr_long[lr]:
t,r = tfm.transform.translation, tfm.transform.rotation
trans = (t.x,t.y,t.z)
rot = (r.x, r.y, r.z, r.w)
hyd_tfm = tfm_c1_h.dot(conversions.trans_rot_to_hmat(trans, rot))
stamp = tfm.header.stamp.to_sec()
tt_tfm = grippers[lr].get_tooltip_transform(lr_long[lr], hyd_tfm)
data[lr]['hydra'].append((tt_tfm, stamp))
found += lr
if found and verbose:
blueprint("Got hydra readings %s at time %f"%(found,stamp))
if not found_pot_data:
if verbose:
yellowprint('Potentiometer readings')
for lr in grippers:
for (_, msg, ts) in bag.read_messages(topics=['/%s_pot_angle'%lr]):
angle = msg.data
stamp = ts.to_sec()
data[lr]['pot_angles'].append((angle, stamp))
if verbose:
blueprint("Got a %s potentiometer angle of %f at time %f"%(lr,angle, stamp))
if verbose:
for lr in 'lr':
yellowprint("Gripper %s:"%lr)
for i in range(num_cameras):
yellowprint("Found %i transforms out of %i rgb/rgbd images from camera%i"%(len(data[lr]['camera%i'%(i+1)]), cam_counts[i], i+1))
yellowprint("Found %i transforms from hydra"%len(data[lr]['hydra']))
yellowprint("Found %i potentiometer readings"%len(data[lr]['pot_angles']))
if save_file is None:
save_file = demo_names.data_name
save_filename = osp.join(demo_dir, save_file)
with open(save_filename, 'w') as sfh: cPickle.dump(data, sfh)
yellowprint("Saved %s."%demo_names.data_name)
os.remove(osp.join(demo_dir, demo_names.extract_data_temp))
def try_smaller_pc():
global getMarkers
rospy.init_node('test_ar')
subprocess.call("killall XnSensorServer", shell=True)
grabber = cpr.CloudGrabber()
grabber.startRGBD()
getMarkers = rospy.ServiceProxy("getMarkers", MarkerPositions)
class clickClass ():
x = None
y = None
done = False
def callback(self, event, x, y, flags, param):
if self.done:
return
elif event == cv2.EVENT_LBUTTONDOWN:
self.x = x
self.y = y
self.done = True
range = 100
try:
while True:
rgb, depth = grabber.getRGBD()
xyz = clouds.depth_to_xyz(depth, asus_xtion_pro_f)
click = clickClass()
print "Click on the point at the middle of the AR marker"
cv2.imshow(WIN_NAME, rgb)
cv2.setMouseCallback(WIN_NAME, click.callback)
while not click.done:
cv2.waitKey(100)
x = click.x
y = click.y
bl = (max(0,x-range), max(0,y-range))
tr = (min(rgb.shape[1]-1,x+range), min(rgb.shape[0]-1, y+range))
cv2.circle(rgb, (x, y), 5, (0, 0, 255), -1)
cv2.circle(rgb, tr, 5, (255, 0, 0), -1)
cv2.circle(rgb, bl, 5, (0, 255, 0), -1)
cv2.rectangle(rgb, bl, tr, (0, 0, 255), 1)
cv2.imshow(WIN_NAME, rgb)
cv2.waitKey(3000)
checkRGB = rgb[bl[0]:tr[0],bl[1]:tr[1],:]
checkXYZ = xyz[bl[0]:tr[0],bl[1]:tr[1],:]
tfms = get_ar_transform_id_unorganized(checkXYZ, checkRGB)
print "The found AR markers are:"
print tfms
except KeyboardInterrupt:
print "Done"
def save_init_ar(demo_type, demo_name, ar_marker, save_in_data_dir=True):
"""
Extracts the initializing ar marker transform and stores it.
"""
# Temp file to show that data is already being extracted
global setCalib
demo_dir = osp.join(demo_files_dir, demo_type, demo_name)
calib_file_path = osp.join(demo_dir, demo_names.calib_name)
with open(osp.join(demo_dir, demo_names.camera_types_name)) as fh:
camera_types = yaml.load(fh)
with open(osp.join(demo_dir, demo_names.camera_models_name)) as fh:
camera_models = yaml.load(fh)
cameras = range(1,len(camera_types)+1)
c_frames = {}
for i in cameras:
c_frames[i]= 'camera%i_link'%(i)
video_dirs = {}
for i in cameras:
video_dirs[i] = osp.join(demo_dir, demo_names.video_dir%i)
tfm_c1 = {i:None for i in cameras}
tfm_c1[1] = np.eye(4)
with open(calib_file_path,'r') as fh: calib_data = cPickle.load(fh)
for tfm in calib_data['transforms']:
if tfm['parent'] == c_frames[1] or tfm['parent'] == '/' + c_frames[1]:
for i in cameras:
if tfm['child'] == c_frames[i] or tfm['child'] == '/' + c_frames[i]:
tfm_c1[i] = nlg.inv(tfm_link_rof).dot(tfm['tfm']).dot(tfm_link_rof)
if not all([tfm_c1[s] != None for s in tfm_c1]):
redprint("Calibration does not have required transforms")
return
# We want the ar marker transform from all the relevant cameras
data = {'tfms':{i:None for i in cameras}, 'marker':ar_marker}
for i in cameras:
if verbose:
yellowprint('Camera%i'%i)
if camera_types[i] == "rgbd":
rgb_fnames, depth_fnames, stamps = eu.get_rgbd_names_times(video_dirs[i])
else:
rgb_fnames, stamps = eu.get_rgbd_names_times(video_dirs[i], depth = False)
all_tfms = []
cam_count = 0
if camera_types[i] == "rgbd":
for ind in rgb_fnames:
rgb = cv2.imread(rgb_fnames[ind])
if displayImages:
cv2.imshow(winname, rgb)
cv2.waitKey(1)
assert rgb is not None
depth = cv2.imread(depth_fnames[ind],2)
assert depth is not None
xyz = clouds.depth_to_xyz(depth, asus_xtion_pro_f)
pc = ru.xyzrgb2pc(xyz, rgb, frame_id='', use_time_now=False)
ar_tfms = get_ar_marker_poses(pc,track=True)
if ar_tfms and ar_marker in ar_tfms:
all_tfms.append(tfm_c1[i].dot(ar_tfms[ar_marker]))
cam_count += 1
else:
if setCalib is None:
setCalib = rospy.ServiceProxy("setCalibInfo", SetCalibInfo)
reqCalib.camera_model = camera_models[i]
setCalib(reqCalib)
if verbose:
yellowprint("Changed camera calibration parameters to model %s"%camera_models[i])
for ind in rgb_fnames:
rgb = cv.LoadImage(rgb_fnames[ind])
if displayImages:
cv.ShowImage(winname, rgb)
cv.WaitKey(1)
assert rgb is not None
ar_tfms = get_ar_marker_poses(rgb,use_pc_service=False,track=True)
if ar_tfms and ar_marker in ar_tfms:
all_tfms.append(tfm_c1[i].dot(ar_tfms[ar_marker]))
cam_count += 1
if verbose:
blueprint ("Got %i values for AR Marker %i from camera %i."%(cam_count, ar_marker, i))
if cam_count > 0:
data['tfms'][i] = utils.avg_transform(all_tfms)
save_filename1 = osp.join(demo_dir, save_file)
with open(save_filename1, 'w') as sfh: cPickle.dump(data, sfh)
if save_in_data_dir:
save_filename2 = osp.join(ar_init_dir, ar_init_demo_name)
with open(save_filename2, 'w') as sfh: cPickle.dump(data, sfh)
yellowprint("Saved %s."%save_file)
