cr_calibration.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 PKG = 'cr_calibration' # this package name
00003 import roslib; roslib.load_manifest(PKG)
00004 
00005 import rospy
00006 import geometry_msgs.msg
00007 import sensor_msgs.msg
00008 import sensor_msgs.srv
00009 import cv_bridge
00010 import cv
00011 import tf
00012 import message_filters
00013 import tf_conversions.posemath as pm
00014 import PyKDL
00015 
00016 import math
00017 import os
00018 import sys
00019 import operator
00020 import time
00021 import numpy
00022 
00023 class CrCalibration:
00024     def __init__(self, size, dim):
00025 
00026         self.chess_size = size
00027         self.dim = dim
00028 
00029         left_ns = rospy.remap_name('left')
00030         range_ns = rospy.remap_name('range')
00031         limg_sub = message_filters.Subscriber(left_ns + '/image_rect', sensor_msgs.msg.Image)
00032         rimg_sub = message_filters.Subscriber(range_ns + '/image_rect', sensor_msgs.msg.Image)
00033         linfo_sub = message_filters.Subscriber(left_ns + '/camera_info',sensor_msgs.msg.CameraInfo)
00034         rinfo_sub = message_filters.Subscriber(range_ns + '/camera_info', sensor_msgs.msg.CameraInfo)
00035 
00036         ts = message_filters.TimeSynchronizer([limg_sub, linfo_sub, rimg_sub, rinfo_sub], 16)
00037         ts.registerCallback(self.queue_cr)
00038         self.bridge = cv_bridge.CvBridge()
00039 
00040         self.frame_list = []
00041         self.static_pose = None
00042         self.result_list = []
00043         self.last_err = 0
00044  
00045     def calc_frame(self, lst):
00046         pos = PyKDL.Vector()
00047         ex = 0
00048         ey = 0
00049         ez = 0
00050         for f in lst:
00051             pos += f.p
00052             z, y, x = f.M.GetEulerZYX()
00053             ex += x
00054             ey += y
00055             ez += z
00056 
00057         size = len(lst)
00058         return PyKDL.Frame(PyKDL.Rotation.EulerZYX(ez/size, ey/size, ex/size), pos/size)
00059 
00060     def calc_distance(self, f1, f2):
00061         test = f1 * f2.Inverse()
00062         angle, axis = test.M.GetRotAngle()
00063         norm = test.p.Norm()
00064         return (norm + (angle / 10.0))
00065 
00066     def queue_cr(self, limg, linfo, rimg, rinfo):
00067         #
00068         lpose = self.find_checkerboard_pose(limg, linfo)
00069         rpose = self.find_checkerboard_pose(rimg, rinfo)
00070         if lpose == None or rpose == None:
00071             if lpose == None:
00072                 rospy.loginfo("can't find CB on left camera image")
00073             if rpose == None:
00074                 rospy.loginfo("can't find CB on range image")
00075             return False
00076 
00077         # calcurate pose
00078         lframe = pm.fromMsg(lpose.pose)
00079         rframe = pm.fromMsg(rpose.pose)
00080         frame = lframe * rframe.Inverse()
00081 
00082         # check pose euniqness
00083         for r in self.result_list:
00084             if self.calc_distance(r[0], lframe) < 0.075:
00085                 return False
00086 
00087         # check pose_movement
00088         if len(self.frame_list) == 0:
00089             self.static_pose = lframe
00090             self.frame_list.append(frame)
00091             return False
00092 
00093         dist = self.calc_distance(self.static_pose, lframe)
00094         print dist
00095         if dist > 0.012:
00096             self.frame_list = []
00097             self.static_pose = None
00098             return False
00099         self.frame_list.append(frame)
00100 
00101         if len(self.frame_list) > 5:
00102             self.result_list.append([self.static_pose, self.calc_frame(self.frame_list)])
00103             self.frame_list = []
00104             self.static_pose = None
00105             # check resut list num
00106             ret = self.calc_frame([r[1] for r in self.result_list])
00107             err = 0.0
00108             for r in self.result_list:
00109                 err += self.calc_distance(r[1], ret)
00110 
00111             qx, qy, qz, qw = ret.M.GetQuaternion()
00112             rospy.loginfo("%f %f %f %f %f %f %f / err = %f" % (ret.p.x(), ret.p.y(), ret.p.z(),
00113                                                                qx, qy, qz, qw, err/len(self.result_list)))
00114             self.last_err = err/len(self.result_list)
00115 
00116             # finish check
00117             if len(self.result_list) > 7 and self.last_err < 0.1:
00118                 rospy.loginfo("Finished size = %d, err = %f" % (len(self.result_list), self.last_err))
00119                 print "translation: [%f, %f, %f]\nrotation: [%f, %f, %f, %f]" % (ret.p.x(), ret.p.y(), ret.p.z(), qx, qy, qz, qw)
00120                 print "(make-coords :pos #f(%f %f %f) :rot (quaternion2matrix #f(%f %f %f %f)))" % (1000*ret.p.x(), 1000*ret.p.y(), 1000*ret.p.z(), qw, qx, qy, qz)
00121                 #print "<node pkg=\"tf\" type=\"static_transform_publisher\" name=\"cam_link_broadcaster\" args=\"%f %f %f %f %f %f %f link1 link2 30\" />" % (ret.p.x(), ret.p.y(), ret.p.z(), qw, qx, qy, qz)
00122                 exit(-1)
00123 
00124             return True
00125 
00126         return True
00127 
00128     def detect(self, image):
00129         corners_x = self.chess_size[0]
00130         corners_y = self.chess_size[1]
00131 
00132         #Here, we'll actually call the openCV detector
00133         found, corners = cv.FindChessboardCorners(image, (corners_x, corners_y), cv.CV_CALIB_CB_ADAPTIVE_THRESH)
00134 
00135         if found:
00136             board_corners = (corners[0],
00137                              corners[corners_x  - 1],
00138                              corners[(corners_y - 1) * corners_x],
00139                              corners[len(corners) - 1])
00140 
00141             #find the perimeter of the checkerboard
00142             perimeter = 0.0
00143             for i in range(len(board_corners)):
00144                 next = (i + 1) % 4
00145                 xdiff = board_corners[i][0] - board_corners[next][0]
00146                 ydiff = board_corners[i][1] - board_corners[next][1]
00147                 perimeter += math.sqrt(xdiff * xdiff + ydiff * ydiff)
00148 
00149             #estimate the square size in pixels
00150             square_size = perimeter / ((corners_x - 1 + corners_y - 1) * 2)
00151             radius = int(square_size * 0.5 + 0.5)
00152 
00153             corners = cv.FindCornerSubPix(image, corners, (radius, radius), (-1, -1), (cv.CV_TERMCRIT_EPS | cv.CV_TERMCRIT_ITER, 30, 0.1))
00154 
00155             #uncomment to debug chessboard detection
00156             #cv.DrawChessboardCorners(image, (corners_x, corners_y), corners, 1)
00157             #cv.NamedWindow("image")
00158             #cv.ShowImage("image", image)
00159             #cv.WaitKey(600)
00160 
00161             #we'll also generate the object points if the user has specified spacing
00162             object_points = cv.CreateMat(3, corners_x * corners_y, cv.CV_32FC1)
00163 
00164             for y in range(corners_y):
00165                 for x in range(corners_x):
00166                     cv.SetReal2D(object_points, 0, y*corners_x + x, x * self.dim)
00167                     cv.SetReal2D(object_points, 1, y*corners_x + x, y * self.dim)
00168                     cv.SetReal2D(object_points, 2, y*corners_x + x, 0.0)
00169 
00170             #not sure why opencv functions return non opencv compatible datatypes... but they do so we'll convert
00171             corners_cv = cv.CreateMat(2, corners_x * corners_y, cv.CV_32FC1)
00172             for i in range(corners_x * corners_y):
00173                 cv.SetReal2D(corners_cv, 0, i, corners[i][0])
00174                 cv.SetReal2D(corners_cv, 1, i, corners[i][1])
00175 
00176             return (corners_cv, object_points)
00177         else:
00178             #cv.NamedWindow("image_scaled")
00179             #cv.ShowImage("image_scaled", image_scaled)
00180             #cv.WaitKey(600)
00181             rospy.logwarn("Didn't find checkerboard")
00182             return (None, None)
00183         return
00184 
00185     def intrinsic_matrix_from_info(self, cam_info):
00186         intrinsic_matrix = cv.CreateMat(3, 3, cv.CV_32FC1)
00187 
00188         #Because we only want the upper 3x3 (normal) portion of the rectified intrinsic matrix
00189         for i in range(0, 3):
00190             for j in range(0, 3):
00191                 intrinsic_matrix[i, j] = cam_info.P[4*i+j]
00192         return intrinsic_matrix
00193 
00194     def find_checkerboard_pose(self, ros_image, cam_info):
00195     #we need to convert the ros image to an opencv image
00196         try:
00197             image = self.bridge.imgmsg_to_cv(ros_image, "mono8")
00198         except CvBridgeError, e:
00199             rospy.logerror("Error importing image %s" % e)
00200             return
00201 
00202         corners, model = self.detect(image)
00203 
00204         if corners == None or model == None:
00205             return None
00206         else:
00207             #find the pose of the checkerboard
00208             rot = cv.CreateMat(3, 1, cv.CV_32FC1)
00209             trans = cv.CreateMat(3, 1, cv.CV_32FC1)
00210             kc = cv.CreateMat(1, 4, cv.CV_32FC1)
00211             cv.Set(kc, 0.0)
00212             intrinsic_matrix = self.intrinsic_matrix_from_info(cam_info)
00213 
00214             cv.FindExtrinsicCameraParams2(model, corners, intrinsic_matrix, kc, rot, trans)
00215 
00216             #We want to build a transform now, but first we have to convert the
00217             #rotation vector we get back from OpenCV into a rotation matrix
00218             rot_mat = cv.CreateMat(3, 3, cv.CV_32FC1)
00219             cv.Rodrigues2(rot, rot_mat)
00220 
00221             #Now we need to convert this rotation matrix into a quaternion
00222             #This can be done better in never versions of opencv, but we need to be boxturtle compatible
00223             numpy_mat = numpy.fromstring(rot_mat.tostring(), dtype = numpy.float32).reshape((3,3))
00224 
00225             #of course, tf expects all matricies passed to it to be 4x4... so we'll build a full matrix here
00226             full_pose = numpy.zeros((4, 4))
00227 
00228             #first, copy in the rotation matrix
00229             full_pose[0:3, 0:3] = numpy_mat[0:3, 0:3]
00230 
00231             #next, we'll copy in the translation
00232             full_pose[0:3, 3] = [trans[i, 0] for i in range(3)]
00233 
00234             #and make sure to add a 1 in the lower right corner
00235             full_pose[3][3] = 1.0
00236 
00237             rospy.logdebug("%s" % numpy_mat)
00238             rospy.logdebug("%s" % full_pose)
00239 
00240             tf_trans = tf.transformations.translation_from_matrix(full_pose)
00241             tf_rot = tf.transformations.quaternion_from_matrix(full_pose)
00242 
00243             #and now we'll actually build our pose stamped
00244             board_pose = geometry_msgs.msg.PoseStamped()
00245             board_pose.header = ros_image.header
00246             board_pose.pose.position.x = tf_trans[0]
00247             board_pose.pose.position.y = tf_trans[1]
00248             board_pose.pose.position.z = tf_trans[2]
00249             board_pose.pose.orientation.x = tf_rot[0]
00250             board_pose.pose.orientation.y = tf_rot[1]
00251             board_pose.pose.orientation.z = tf_rot[2]
00252             board_pose.pose.orientation.w = tf_rot[3]
00253             rospy.logdebug("%s" % board_pose)
00254 
00255             #we'll publish the pose so we can display it in rviz
00256             # self.pose_pub.publish(board_pose)
00257             return board_pose
00258 
00259 def main():
00260     from optparse import OptionParser
00261     rospy.init_node('cr_calibrator')
00262     parser = OptionParser()
00263     parser.add_option("-s", "--size", default="8x6", help="specify chessboard size as nxm [default: %default]")
00264     parser.add_option("-q", "--square", default=".108", help="specify chessboard square size in meters [default: %default]")
00265 
00266     options, args = parser.parse_args()
00267     size = tuple([int(c) for c in options.size.split('x')])
00268     dim = float(options.square)
00269     node = CrCalibration(size, dim)
00270 
00271     rospy.spin()
00272 
00273 if __name__ == "__main__":
00274     main()
00275 


cr_calibration
Author(s): youhei kakiuchi, JSK
autogenerated on Mon Oct 6 2014 01:18:08