def find_image_features(bagname, topic):
features_list = []
bridge = CvBridge()
i = 0
for topic, msg, t in ru.bag_iter(bagname, [topic]):
t = msg.header.stamp.to_time()
image = bridge.imgmsg_to_cv(msg, 'bgr8')
image_gray = fea.grayscale(image)
surf_keypoints, surf_descriptors = fea.surf(image_gray)
features_list.append((t, (surf_keypoints, surf_descriptors)))
rospy.loginfo("%.3f frame %d found %d points" % (t, i, len(surf_keypoints)))
i = i + 1
return features_list
import hrl_lib.rutils as ru
import hrl_lib.tf_utils as tfu
import tf.transformations as tr
import tf
import hrl_camera.ros_camera as cam
from sensor_msgs.msg import CameraInfo
import numpy as np
import hai_sandbox.features as fea
import os.path as pt
import hrl_lib.util as ut
import itertools as it
#Load original pickle
orig_bag = sys.argv[1]
topic = '/l_forearm_cam/image_rect_color'
#Load surf pickle
print 'loading pickle', sys.argv[2]
surf_pkl = ut.load_pickle(sys.argv[2])
new_surf_data = []
print 'replacing time field'
for tmt, surf_record in it.izip(ru.bag_iter(orig_bag, [topic]), surf_pkl) :
topic, msg, t = tmt
surf_t, surf_data = surf_record
new_surf_data.append((msg.header.stamp.to_time(), surf_data))
print 'saving pickle with new time', sys.argv[2]
ut.save_pickle(new_surf_data, sys.argv[2])
images_file = sys.argv[2]
features_db = np.column_stack([ut.load_pickle(p[0]) for p in csv_bag_names(features_file)])
features_db_reduced = features_mat_compress(features_db, 500)
#Generate a random color for each feature
colors = np.matrix(np.random.randint(0, 255, (3, features_db_reduced.shape[1])))
features_tree = sp.KDTree(np.array(features_db_reduced.T))
bridge = CvBridge()
forearm_cam_l = '/l_forearm_cam/image_rect_color'
cv.NamedWindow('surf', 1)
#import pdb
#while not rospy.is_shutdown():
i = 0
for topic, msg, t in ru.bag_iter(images_file, [forearm_cam_l]):
image = bridge.imgmsg_to_cv(msg, 'bgr8')
#image = camera.get_frame()
image_gray = fea.grayscale(image)
surf_keypoints, surf_descriptors = fea.surf(image_gray)
#print len(surf_keypoints)
#pdb.set_trace()
#match each keypoint with one in our db & look up color
matching_idx = [features_tree.query(d)[1] for d in surf_descriptors]
coordinated_colors = colors[:, matching_idx]
#nimage = fea.draw_surf(image, surf_keypoints, (0,255,0))
nimage = fea.draw_surf2(image, surf_keypoints, coordinated_colors)
cv.ShowImage('surf', nimage)
cv.SaveImage('forearm_cam%d.png' % i, nimage)
pass
#contact_mat(pmesg)
if __name__ == '__main__':
import sys
import pdb
fname = sys.argv[1]
scene = None
# Use offline bag files
# Load the pointcloud messages
# Find out the variance in # of points
# Select one as canonical
i = 0
for top, pc, t in ru.bag_iter(fname, ['/full_cloud']):
# Want first message of at least this size
if len(pc.points) > 20000:
if i > 0:
pdb.set_trace()
scene = pointcloud_to_np(pc)
break
i = i + 1
# Run online to get gripper tip locations from TF
# Subscribe to pressure readings, find contact times & get gripper tip locations
ctl = ContactTipLocation()
r = rospy.Rate(10)
print 'running contact tip recorder'
while not rospy.is_shutdown():
r.sleep()
def get_closest_msgs(fname, topics, times):
times_set = set(times)
for top, msg, t in ru.bag_iter(fname, topics):
msg_time = msg.header.stamp.to_time()
if len(times_set.intersection([msg_time])) > 0:
yield msg
import roslib; roslib.load_manifest('hai_sandbox')
from cv_bridge.cv_bridge import CvBridge, CvBridgeError
import cv
import sys
import hrl_lib.rutils as ru
import hai_sandbox.features as fea
forearm_cam_l = '/l_forearm_cam/image_rect_color'
ws_l = '/wide_stereo/left/image_rect_color'
ws_r = '/wide_stereo/right/image_rect_color'
fname = sys.argv[1]
bridge = CvBridge()
cv.NamedWindow('surf', 1)
cv.NamedWindow('harris', 1)
cv.NamedWindow('star', 1)
for topic, msg, t in ru.bag_iter(fname, [ws_l]):
image = bridge.imgmsg_to_cv(msg, 'bgr8')
image_gray = fea.grayscale(image)
surf_keypoints, surf_descriptors = fea.surf(image_gray)
cv.ShowImage('surf', fea.draw_surf(image, surf_keypoints, (255, 0, 0)))
harris_keypoints = fea.harris(image_gray)
cv.ShowImage('harris', fea.draw_harris(image, harris_keypoints, (0, 255, 0)))
cv.WaitKey(10)
#fname_cloud = sys.argv[3] press_lt = '/pressure/l_gripper_motor' press_rt = '/pressure/r_gripper_motor' forearm_cam_l = '/l_forearm_cam/image_rect_color' ws_l = '/wide_stereo/left/image_rect_color' ws_r = '/wide_stereo/right/image_rect_color' cloud_top = '/full_cloud' print 'reading pressure messages' #Get the pressure messages msgs_dict = ru.bag_sel(fname, [press_lt, press_rt]) #Get the image times print 'getting image times' all_cam_times = group_by_first_el([[top, msg.header.stamp.to_time()] for top, msg, t in ru.bag_iter(fname_wide, [ws_l, ws_r])]) all_cam_times[forearm_cam_l] = [[top, msg.header.stamp.to_time()] for top, msg, t in ru.bag_iter(fname, [forearm_cam_l])] [msg.header.stamp.to_time() for top, msg, t in ru.bag_iter(fname, ['/wide_stereo/left/image_raw'])][0:4] print 'processing pressure' press_lmsgs = [msg for top, msg, t in msgs_dict[press_lt]] press_rmsgs = [msg for top, msg, t in msgs_dict[press_rt]] #ll_mat contains (contact_loc, contact_times) ll_mat, lr_mat, times_l = contact_mat(press_lmsgs) rl_mat, rr_mat, times_r = contact_mat(press_rmsgs) contact_loc, times_contact_pressure = find_contact_times(ll_mat, lr_mat, times_l) print 'contact loc', contact_loc #figure out which images are closest in time
def get_closest_msgs(fname, topics, times):
times_set = set(times)
for top, msg, t in ru.bag_iter(fname, topics):
msg_time = msg.header.stamp.to_time()
if len(times_set.intersection([msg_time])) > 0:
yield msg
press_lt = '/pressure/l_gripper_motor'
press_rt = '/pressure/r_gripper_motor'
forearm_cam_l = '/l_forearm_cam/image_rect_color'
ws_l = '/wide_stereo/left/image_rect_color'
ws_r = '/wide_stereo/right/image_rect_color'
cloud_top = '/full_cloud'
print 'reading pressure messages'
#Get the pressure messages
msgs_dict = ru.bag_sel(fname, [press_lt, press_rt])
#Get the image times
print 'getting image times'
all_cam_times = group_by_first_el(
[[top, msg.header.stamp.to_time()]
for top, msg, t in ru.bag_iter(fname_wide, [ws_l, ws_r])])
all_cam_times[forearm_cam_l] = [[
top, msg.header.stamp.to_time()
] for top, msg, t in ru.bag_iter(fname, [forearm_cam_l])]
[
msg.header.stamp.to_time()
for top, msg, t in ru.bag_iter(fname, ['/wide_stereo/left/image_raw'])
][0:4]
print 'processing pressure'
press_lmsgs = [msg for top, msg, t in msgs_dict[press_lt]]
press_rmsgs = [msg for top, msg, t in msgs_dict[press_rt]]
#ll_mat contains (contact_loc, contact_times)
ll_mat, lr_mat, times_l = contact_mat(press_lmsgs)
[ut.load_pickle(p[0]) for p in csv_bag_names(features_file)])
features_db_reduced = features_mat_compress(features_db, 500)
#Generate a random color for each feature
colors = np.matrix(
np.random.randint(0, 255, (3, features_db_reduced.shape[1])))
features_tree = sp.KDTree(np.array(features_db_reduced.T))
bridge = CvBridge()
forearm_cam_l = '/l_forearm_cam/image_rect_color'
cv.NamedWindow('surf', 1)
#import pdb
#while not rospy.is_shutdown():
i = 0
for topic, msg, t in ru.bag_iter(images_file, [forearm_cam_l]):
image = bridge.imgmsg_to_cv(msg, 'bgr8')
#image = camera.get_frame()
image_gray = fea.grayscale(image)
surf_keypoints, surf_descriptors = fea.surf(image_gray)
#print len(surf_keypoints)
#pdb.set_trace()
#match each keypoint with one in our db & look up color
matching_idx = [features_tree.query(d)[1] for d in surf_descriptors]
coordinated_colors = colors[:, matching_idx]
#nimage = fea.draw_surf(image, surf_keypoints, (0,255,0))
nimage = fea.draw_surf2(image, surf_keypoints, coordinated_colors)
cv.ShowImage('surf', nimage)
cv.SaveImage('forearm_cam%d.png' % i, nimage)
import hrl_lib.rutils as ru
import hrl_lib.tf_utils as tfu
import tf.transformations as tr
import tf
import hrl_camera.ros_camera as cam
from sensor_msgs.msg import CameraInfo
import numpy as np
import hai_sandbox.features as fea
import os.path as pt
import hrl_lib.util as ut
import itertools as it
#Load original pickle
orig_bag = sys.argv[1]
topic = '/l_forearm_cam/image_rect_color'
#Load surf pickle
print 'loading pickle', sys.argv[2]
surf_pkl = ut.load_pickle(sys.argv[2])
new_surf_data = []
print 'replacing time field'
for tmt, surf_record in it.izip(ru.bag_iter(orig_bag, [topic]), surf_pkl):
topic, msg, t = tmt
surf_t, surf_data = surf_record
new_surf_data.append((msg.header.stamp.to_time(), surf_data))
print 'saving pickle with new time', sys.argv[2]
ut.save_pickle(new_surf_data, sys.argv[2])
