def image_cb(self, msg): image_time = msg.header.stamp.to_time() image = self.bridge.imgmsg_to_cv(msg, 'bgr8') if self.surf_idx == None: for i, d in enumerate(self.surf_data): t, sdata = d if image_time == t: self.surf_idx = i break else: self.surf_idx = self.surf_idx + 1 stime, sdata = self.surf_data[self.surf_idx] if stime != image_time: print 'surf time != image_time' surf_keypoints, surf_descriptors = sdata nimage = fea.draw_surf(image, surf_keypoints, (255,0,0)) cv.ShowImage('surf', nimage) cv.WaitKey(10) # Track and give 3D location to features. ## Project 3D points into this frame (need access to tf => must do online or from cache) ## camera_T_3dframe at ti scene2d = self.camera_geo.project(self.scene, self.tflistener, 'base_footprint') scene2d_tree = sp.KDTree(np.array(scene2d.T)) ## Find features close to these 2d points for loc, lap, size, d, hess in surf_keypoints: idx = scene2d_tree.query(np.array(loc))[1] orig3d = self.scene[:, idx] ## Get features closest to the contact point ## stop running if contact has stopped if self.contact:
def run(self): cv.NamedWindow('surf', 1) while not rospy.is_shutdown(): image = self.camera.get_frame() image_gray = fea.grayscale(image) surf_keypoints, surf_descriptors = fea.surf(image_gray) vis_img = fea.draw_surf(image, surf_keypoints, (255, 0, 0)) #Project the tip of the gripper (both of them) into the image frame img_ll = self.camera_geo.project(np.matrix([0,0,0.]).T, self.tflistener, self.finger_tips[2]) img_lr = self.camera_geo.project(np.matrix([0,0,0.]).T, self.tflistener, self.finger_tips[3]) cv.Circle(vis_img, tuple(np.matrix(np.round(img_ll), dtype='int').A1.tolist()), 30, (0, 255, 0), 1, cv.CV_AA) cv.Circle(vis_img, tuple(np.matrix(np.round(img_lr), dtype='int').A1.tolist()), 30, (0, 255, 0), 1, cv.CV_AA) cv.ShowImage('surf', vis_img) cv.WaitKey(10)
def run(self): cv.NamedWindow('surf', 1) while not rospy.is_shutdown(): image = self.camera.get_frame() image_gray = fea.grayscale(image) surf_keypoints, surf_descriptors = fea.surf(image_gray) vis_img = fea.draw_surf(image, surf_keypoints, (255, 0, 0)) #Project the tip of the gripper (both of them) into the image frame img_ll = self.camera_geo.project( np.matrix([0, 0, 0.]).T, self.tflistener, self.finger_tips[2]) img_lr = self.camera_geo.project( np.matrix([0, 0, 0.]).T, self.tflistener, self.finger_tips[3]) cv.Circle( vis_img, tuple(np.matrix(np.round(img_ll), dtype='int').A1.tolist()), 30, (0, 255, 0), 1, cv.CV_AA) cv.Circle( vis_img, tuple(np.matrix(np.round(img_lr), dtype='int').A1.tolist()), 30, (0, 255, 0), 1, cv.CV_AA) cv.ShowImage('surf', vis_img) cv.WaitKey(10)
import roslib roslib.load_manifest('hai_sandbox') import cv import hai_sandbox.features as fea import hrl_camera.ros_camera as rc import rospy #prosilica = rc.Prosilica('prosilica', 'streaming') #prosilica = rc.ROSCamera('/narrow_stereo/right/image_rect') prosilica = rc.ROSCamera('/wide_stereo/right/image_rect_color') cv.NamedWindow('surf', 1) while not rospy.is_shutdown(): f = prosilica.get_frame() loc, desc = fea.surf_color(f, params=(0, 3000, 3, 4)) fdrawn = fea.draw_surf(f, loc, (0, 255, 0)) cv.ShowImage('surf', fdrawn) cv.WaitKey(33)
import roslib; roslib.load_manifest('hai_sandbox') import cv import sys import hai_sandbox.features as fea if __name__ == '__main__': fname = sys.argv[1] image = cv.LoadImage(fname) image_gray = cv.CreateImage((640,480), cv.IPL_DEPTH_8U,1) cv.CvtColor(image, image_gray, cv.CV_BGR2GRAY) star_keypoints = fea.star(image) surf_keypoints, surf_descriptors = fea.surf(image_gray) harris_keypoints = fea.harris(image_gray) cv.NamedWindow('surf', 1) cv.NamedWindow('harris', 1) cv.NamedWindow('star', 1) while True: cv.ShowImage('surf', fea.draw_surf(image, surf_keypoints, (255, 0, 0))) cv.ShowImage('harris', fea.draw_harris(image, harris_keypoints, (0, 255, 0))) cv.ShowImage('star', fea.draw_star(image, star_keypoints, (0, 0, 255))) k = cv.WaitKey(33) if k == 27: break #Canny(image, edges, threshold1, threshold2, aperture_size=3) => None
while not rospy.is_shutdown(): print '..' image = prosilica.get_frame() print 'saving image' cv.SaveImage('frame.png', image) print '>' img_gray = fea.grayscale(image) locs, descs = fea.surf(img_gray) match_idxs = [] for i, desc in enumerate(descs): dists, idxs = features_db.query(np.array(desc), 2) ratio = dists[0] / dists[1] if ratio < .49: match_idxs.append(i) img_viz = fea.draw_surf(image, locs, (255,0,0)) img_viz = fea.draw_surf(img_viz, [locs[i] for i in match_idxs], (0,0,255)) si.image = img_viz print '%d matches found' % len(match_idxs) #print len(desc), desc.__class__, len(descs[0]), descs[0].__class__ #si.image = image
except Exception, e: print e print 'listening' et = ListenAndFindContactLocs() r = rospy.Rate(10) while not rospy.is_shutdown() and not et.contact_stopped: r.sleep() contact_locs = et.contact_locs ut.save_pickle(et.contact_locs, 'contact_locs_proc.pkl') #Detect features, get 3d location for each feature print 'detecting features' proc_img = cv.LoadImage(proc_img_name) proc_gray = fea.grayscale(proc_img) sloc, sdesc = fea.surf(proc_gray) proc_surfed = fea.draw_surf(proc_img, sloc, (200, 0, 0)) ###################################################################################### # get 3d locations of surf features, get closest surf feature to gripper tips ###################################################################################### #import pdb #pdb.set_trace() point_cloud_pro = data_dict['pro_T_bf'] * np.row_stack( (point_cloud, 1 + np.zeros((1, point_cloud.shape[1])))) #project 3d points to 2d point_cloud_2d = data_dict['camera_info'].project(point_cloud_pro[0:3, :]) point_cloud_2d_tree = sp.KDTree(np.array(point_cloud_2d.T)) #2d surf => 3d loc surf_loc3d = [] for loc, lap, size, d, hess in sloc:
import roslib roslib.load_manifest("hai_sandbox") import cv import hai_sandbox.features as fea import hrl_camera.ros_camera as rc import rospy # prosilica = rc.Prosilica('prosilica', 'streaming') # prosilica = rc.ROSCamera('/narrow_stereo/right/image_rect') prosilica = rc.ROSCamera("/wide_stereo/right/image_rect_color") cv.NamedWindow("surf", 1) while not rospy.is_shutdown(): f = prosilica.get_frame() loc, desc = fea.surf_color(f, params=(0, 3000, 3, 4)) fdrawn = fea.draw_surf(f, loc, (0, 255, 0)) cv.ShowImage("surf", fdrawn) cv.WaitKey(33)
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)
except Exception, e: print e print 'listening' et = ListenAndFindContactLocs() r = rospy.Rate(10) while not rospy.is_shutdown() and not et.contact_stopped: r.sleep() contact_locs = et.contact_locs ut.save_pickle(et.contact_locs, 'contact_locs_proc.pkl') #Detect features, get 3d location for each feature print 'detecting features' proc_img = cv.LoadImage(proc_img_name) proc_gray = fea.grayscale(proc_img) sloc, sdesc = fea.surf(proc_gray) proc_surfed = fea.draw_surf(proc_img, sloc, (200, 0, 0)) ###################################################################################### # get 3d locations of surf features, get closest surf feature to gripper tips ###################################################################################### #import pdb #pdb.set_trace() point_cloud_pro = data_dict['pro_T_bf'] * np.row_stack((point_cloud, 1+np.zeros((1, point_cloud.shape[1])))) #project 3d points to 2d point_cloud_2d = data_dict['camera_info'].project(point_cloud_pro[0:3,:]) point_cloud_2d_tree = sp.KDTree(np.array(point_cloud_2d.T)) #2d surf => 3d loc surf_loc3d = [] for loc, lap, size, d, hess in sloc: idx = point_cloud_2d_tree.query(np.array(loc))[1]