00001
00002
00003 import copy
00004 import pygame
00005 import pygame.image
00006 import numpy as np
00007
00008 import roslib
00009 roslib.load_manifest('UI_segment_object')
00010 roslib.load_manifest('camera_calibration')
00011 roslib.load_manifest('pixel_2_3d')
00012 roslib.load_manifest('hrl_generic_arms')
00013
00014 import cv
00015
00016 import rospy
00017 from sensor_msgs.msg import Image
00018 from cv_bridge import CvBridge, CvBridgeError
00019 import tf
00020
00021 from camera_calibration.calibrator import ChessboardInfo, Calibrator
00022 from pixel_2_3d.srv import Pixel23d
00023 from hrl_generic_arms.pose_converter import PoseConverter
00024
00025 class ImageListener:
00026 def __init__(self, topic):
00027 self.bridge = CvBridge()
00028 self.image_sub = rospy.Subscriber(topic, Image, self.callback)
00029 self.msg_img = None
00030
00031 def callback(self, data):
00032 try:
00033 self.msg_img = data
00034 except CvBridgeError, e:
00035 print e
00036 return
00037
00038 def get_cv_img(self):
00039 if self.msg_img is None:
00040 return None
00041 cv_img = self.bridge.imgmsg_to_cv(self.msg_img, "rgb8")
00042 return cv_img
00043
00044 def get_pg_img(self, cv_img):
00045 return pygame.image.frombuffer(cv_img.tostring(),
00046 cv.GetSize(cv_img), "RGB")
00047
00048 def umeyama_method(A, B):
00049 ux = np.mean(A, 1)
00050 uy = np.mean(B, 1)
00051 A_demean = A - ux
00052 B_demean = B - uy
00053 U, D, VT = np.linalg.svd(A_demean * B_demean.T)
00054 R = VT.T * U.T
00055 t = uy - R * ux
00056 return t, R
00057
00058 def ransac(A, B, err_thresh, percent_set_train=0.5, percent_set_fit=0.5, iterations=200):
00059 best_model = None
00060 best_consen_set = None
00061 best_error = 1000000.0
00062 n = int(percent_set_train * A.shape[1])
00063 min_num = int(percent_set_fit * A.shape[1])
00064 for i in range(iterations):
00065 shuff_inds = np.arange(A.shape[1])
00066 np.random.shuffle(shuff_inds)
00067 maybe_inds = shuff_inds[:n]
00068 maybe_A = A[:, maybe_inds]
00069 maybe_B = B[:, maybe_inds]
00070 t_maybe, R_maybe = umeyama_method(maybe_A, maybe_B)
00071 err = (R_maybe * A + t_maybe - B).A
00072 err_mag = np.sqrt(err[0,:] ** 2 + err[1,:] ** 2 + err[2,:] ** 2)
00073 if np.sum(err_mag < err_thresh) < min_num:
00074 print "Not enough points within thresh"
00075 continue
00076 consensus_A = A[:, err_mag < err_thresh]
00077 consensus_B = B[:, err_mag < err_thresh]
00078 t_cons, R_cons = umeyama_method(consensus_A, consensus_B)
00079 err_cons = (R_cons * consensus_A + t_cons - consensus_B).A
00080 err_mag_cons = np.sqrt(err_cons[0,:] ** 2 + err_cons[1,:] ** 2 + err_cons[2,:] ** 2)
00081 if np.mean(err_mag_cons) < best_error:
00082 best_error = np.mean(err_mag_cons)
00083 best_model = (t_cons, R_cons)
00084 print "Best error:", best_error
00085 print "Consensus Size:", consensus_A.shape[1]
00086 return best_model
00087
00088 def main():
00089 gray = (100,100,100)
00090 corner_len = 5
00091 chessboard = ChessboardInfo()
00092 chessboard.n_cols = 6
00093 chessboard.n_rows = 7
00094 chessboard.dim = 0.02273
00095 cboard_frame = "kinect_cb_corner"
00096
00097
00098 use_pygame = False
00099 kinect_tracker_frame = "pr2_antenna"
00100
00101 rospy.init_node("kinect_calib_test")
00102 img_list = ImageListener("/kinect_head/rgb/image_color")
00103 pix3d_srv = rospy.ServiceProxy("/pixel_2_3d", Pixel23d, True)
00104 tf_list = tf.TransformListener()
00105 if use_pygame:
00106 pygame.init()
00107 clock = pygame.time.Clock()
00108 screen = pygame.display.set_mode((640, 480))
00109 calib = Calibrator([chessboard])
00110 done = False
00111 corner_list = np.ones((2, corner_len)) * -1000.0
00112 corner_i = 0
00113 saved_corners_2d, saved_corners_3d, cb_locs = [], [], []
00114 while not rospy.is_shutdown():
00115 try:
00116 cb_pos, cb_quat = tf_list.lookupTransform(kinect_tracker_frame,
00117 cboard_frame,
00118 rospy.Time())
00119 except:
00120 rospy.sleep(0.001)
00121 continue
00122 cv_img = img_list.get_cv_img()
00123 if cv_img is not None:
00124 has_corners, corners, chess = calib.get_corners(cv_img)
00125 for corner2d in saved_corners_2d:
00126 cv.Circle(cv_img, corner2d, 4, [0, 255, 255])
00127 if has_corners:
00128 corner_i += 1
00129 corner = corners[0]
00130 if use_pygame:
00131 for event in pygame.event.get():
00132 if event.type == pygame.KEYDOWN:
00133 print event.dict['key'], pygame.K_d
00134 if event.dict['key'] == pygame.K_d:
00135 done = True
00136 if event.dict['key'] == pygame.K_q:
00137 return
00138 if done:
00139 break
00140 corner_list[:, corner_i % corner_len] = corner
00141 if np.linalg.norm(np.var(corner_list, 1)) < 1.0:
00142 corner_avg = np.mean(corner_list, 1)
00143 corner_avg_tuple = tuple(corner_avg.round().astype(int).tolist())
00144 cv.Circle(cv_img, corner_avg_tuple, 4, [0, 255, 0])
00145 pix3d_resp = pix3d_srv(*corner_avg_tuple)
00146 if pix3d_resp.error_flag == pix3d_resp.SUCCESS:
00147 corner_3d_tuple = (pix3d_resp.pixel3d.pose.position.x,
00148 pix3d_resp.pixel3d.pose.position.y,
00149 pix3d_resp.pixel3d.pose.position.z)
00150 if len(saved_corners_3d) == 0:
00151 cb_locs.append(cb_pos)
00152 saved_corners_2d.append(corner_avg_tuple)
00153 saved_corners_3d.append(corner_3d_tuple)
00154 else:
00155 diff_arr = np.array(np.mat(saved_corners_3d) - np.mat(corner_3d_tuple))
00156 if np.min(np.sqrt(np.sum(diff_arr ** 2, 1))) >= 0.03:
00157 cb_locs.append(cb_pos)
00158 saved_corners_2d.append(corner_avg_tuple)
00159 saved_corners_3d.append(corner_3d_tuple)
00160 print "Added sample", len(saved_corners_2d) - 1
00161 else:
00162 cv.Circle(cv_img, corner, 4, [255, 0, 0])
00163 else:
00164 corner_list = np.ones((2, corner_len)) * -1000.0
00165 if use_pygame:
00166 if cv_img is None:
00167 screen.fill(gray)
00168 else:
00169 screen.blit(img_list.get_pg_img(cv_img), (0, 0))
00170 pygame.display.flip()
00171 rospy.sleep(0.001)
00172 A = np.mat(saved_corners_3d).T
00173 B = np.mat(cb_locs).T
00174 print A, B
00175 t, R = umeyama_method(A, B)
00176 print A, B, R, t
00177 print "-" * 60
00178 print "Transformation Parameters:"
00179 pos, quat = PoseConverter.to_pos_quat(t, R)
00180 print '%f %f %f %f %f %f %f' % tuple(pos + quat)
00181 t_r, R_r = ransac(A, B, 0.02, percent_set_train=0.5, percent_set_fit=0.6)
00182 print t_r, R_r
00183 pos, quat = PoseConverter.to_pos_quat(t_r, R_r)
00184 print '%f %f %f %f %f %f %f' % tuple(pos + quat)
00185
00186 if __name__ == '__main__':
00187 main()