00001 import roslib; roslib.load_manifest('hai_sandbox')
00002 from cv_bridge.cv_bridge import CvBridge, CvBridgeError
00003 import rospy
00004 import cv
00005 import sys
00006
00007 import hrl_lib.tf_utils as tfu
00008 import tf.transformations as tr
00009 import tf
00010 import hrl_camera.ros_camera as cam
00011 from sensor_msgs.msg import CameraInfo
00012 import numpy as np
00013 import hai_sandbox.features as fea
00014
00015
00016
00017
00018 class ROSCameraCalibration:
00019 def __init__(self, channel):
00020 rospy.Subscriber(channel, CameraInfo, self.camera_info)
00021 self.has_msg = False
00022
00023 def camera_info(self, msg):
00024 self.distortion = np.matrix(msg.D)
00025 self.K = np.reshape(np.matrix(msg.K), (3,3))
00026 self.R = np.reshape(np.matrix(msg.R), (3,3))
00027 self.P = np.reshape(np.matrix(msg.P), (3,4))
00028 self.w = msg.width
00029 self.h = msg.height
00030 self.frame = msg.header.frame_id
00031 self.has_msg = True
00032
00033
00034
00035
00036
00037
00038
00039
00040 def project(self, p, tf_listener=None, from_frame=None):
00041 if not(from_frame == None or from_frame == self.frame):
00042 p_cam = tfu.transform(self.frame, from_frame, tf_listener) \
00043 * tfu.tf_as_matrix((p.A1.tolist(), tr.quaternion_from_euler(0,0,0)))
00044 trans, q = tfu.matrix_as_tf(p_cam)
00045 p = np.matrix(trans).T
00046
00047 p = np.row_stack((p, np.matrix([1.])))
00048 pp = self.P * p
00049 pp = pp / pp[2,0]
00050 return pp[0:2,0]
00051
00052
00053 class GripperTipProjected:
00054 def __init__(self):
00055 forearm_cam_l = '/l_forearm_cam/image_rect_color'
00056 ws_l = '/wide_stereo/left/image_rect_color'
00057 ws_r = '/wide_stereo/right/image_rect_color'
00058 ws_linf = '/wide_stereo/left/camera_info'
00059 ws_rinf = '/wide_stereo/right/camera_info'
00060
00061 self.finger_tips = ['r_gripper_l_finger_tip_link',
00062 'r_gripper_r_finger_tip_link',
00063 'l_gripper_l_finger_tip_link',
00064 'l_gripper_r_finger_tip_link']
00065
00066 self.camera_fr = ['r_forearm_cam_optical_frame',
00067 'l_forearm_cam_optical_frame',
00068 'wide_stereo_optical_frame']
00069
00070 rospy.init_node('gripper_pose_viewer')
00071
00072 self.camera_geo = ROSCameraCalibration('/l_forearm_cam/camera_info')
00073 self.camera = cam.ROSImageClient(forearm_cam_l)
00074 self.tflistener = tf.TransformListener()
00075
00076
00077 def run(self):
00078 cv.NamedWindow('surf', 1)
00079 while not rospy.is_shutdown():
00080 image = self.camera.get_frame()
00081 image_gray = fea.grayscale(image)
00082 surf_keypoints, surf_descriptors = fea.surf(image_gray)
00083 vis_img = fea.draw_surf(image, surf_keypoints, (255, 0, 0))
00084
00085
00086 img_ll = self.camera_geo.project(np.matrix([0,0,0.]).T, self.tflistener, self.finger_tips[2])
00087 img_lr = self.camera_geo.project(np.matrix([0,0,0.]).T, self.tflistener, self.finger_tips[3])
00088
00089 cv.Circle(vis_img, tuple(np.matrix(np.round(img_ll), dtype='int').A1.tolist()), 30, (0, 255, 0), 1, cv.CV_AA)
00090 cv.Circle(vis_img, tuple(np.matrix(np.round(img_lr), dtype='int').A1.tolist()), 30, (0, 255, 0), 1, cv.CV_AA)
00091 cv.ShowImage('surf', vis_img)
00092 cv.WaitKey(10)
00093
00094
00095 if __name__ == '__main__':
00096 g = GripperTipProjected()
00097 g.run()
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108