Пример #1
0
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
Пример #2
0
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])
Пример #3
0
    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)
Пример #4
0
        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()
Пример #5
0
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
Пример #6
0
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)
    
Пример #7
0
#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
Пример #8
0
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
Пример #9
0
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)
Пример #10
0
        [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)
Пример #11
0
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])