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]
