Go to the documentation of this file.00001 import roslib; roslib.load_manifest('hai_sandbox')
00002 import rospy
00003 import hrl_lib.util as ut
00004 import csv
00005 import scipy.spatial as sp
00006 import hrl_camera.ros_camera as cam
00007 import hai_sandbox.features as fea
00008 import numpy as np
00009 import cv
00010 import hrl_lib.rutils as ru
00011 from cv_bridge.cv_bridge import CvBridge, CvBridgeError
00012 import scipy.cluster.vq as vq
00013
00014 def csv_bag_names(fname):
00015 csv_file = open(fname)
00016 for bag_name in csv.reader(csv_file):
00017 yield bag_name
00018 csv_file.close()
00019
00020 def features_mat_compress(fmat, k):
00021
00022 rospy.loginfo('compressing to %d centers' % k)
00023 center_indices = np.random.permutation(fmat.shape[1])[0:k]
00024 initial_centers = fmat[:, center_indices]
00025 kresults = vq.kmeans(np.array(fmat.T), np.array(initial_centers.T))
00026 return np.matrix(kresults[0]).T
00027
00028 if __name__ == '__main__':
00029 import sys
00030 import pdb
00031 features_file = sys.argv[1]
00032 images_file = sys.argv[2]
00033 features_db = np.column_stack([ut.load_pickle(p[0]) for p in csv_bag_names(features_file)])
00034 features_db_reduced = features_mat_compress(features_db, 500)
00035
00036
00037 colors = np.matrix(np.random.randint(0, 255, (3, features_db_reduced.shape[1])))
00038 features_tree = sp.KDTree(np.array(features_db_reduced.T))
00039 bridge = CvBridge()
00040
00041 forearm_cam_l = '/l_forearm_cam/image_rect_color'
00042 cv.NamedWindow('surf', 1)
00043
00044
00045
00046 i = 0
00047 for topic, msg, t in ru.bag_iter(images_file, [forearm_cam_l]):
00048 image = bridge.imgmsg_to_cv(msg, 'bgr8')
00049
00050 image_gray = fea.grayscale(image)
00051 surf_keypoints, surf_descriptors = fea.surf(image_gray)
00052
00053
00054
00055
00056 matching_idx = [features_tree.query(d)[1] for d in surf_descriptors]
00057 coordinated_colors = colors[:, matching_idx]
00058
00059
00060 nimage = fea.draw_surf2(image, surf_keypoints, coordinated_colors)
00061 cv.ShowImage('surf', nimage)
00062 cv.SaveImage('forearm_cam%d.png' % i, nimage)
00063 i = i + 1
00064 cv.WaitKey(10)
00065
00066
00067
00068
00069
00070
00071
00072