$search
00001 #! /usr/bin/env python 00002 00003 import roslib 00004 roslib.load_manifest('camera_pose_calibration') 00005 00006 import itertools 00007 import collections 00008 import rospy 00009 import threading 00010 import tf 00011 00012 import math 00013 00014 from tf_conversions import posemath 00015 from std_msgs.msg import Empty 00016 from camera_pose_calibration.msg import CalibrationEstimate 00017 from camera_pose_calibration.msg import CameraPose 00018 from camera_pose_calibration.msg import RobotMeasurement, CameraCalibration 00019 from camera_pose_calibration import init_optimization_prior 00020 from camera_pose_calibration import estimate 00021 from camera_pose_calibration import camera_info_converter 00022 from std_msgs.msg import Time 00023 from sensor_msgs.msg import CameraInfo 00024 00025 class Estimator: 00026 def __init__(self): 00027 self.lock = threading.Lock() 00028 self.meas = [] ## 00029 self.reset() 00030 00031 00032 self.prev_tf_transforms = {} 00033 self.tf_check_pairs = [] 00034 self.tf_listener = tf.TransformListener() 00035 00036 self.measurement_count = 0 00037 self.timestamps =[] 00038 00039 self.pub = rospy.Publisher('camera_calibration', CameraCalibration) 00040 self.sub_reset = rospy.Subscriber('reset', Empty, self.reset_cb) 00041 self.sub_meas = rospy.Subscriber('robot_measurement', RobotMeasurement, self.meas_cb) 00042 00043 self.prev_tf_pair=() 00044 self.prev_3_frames=() 00045 self.reset_flag = False 00046 self.frames_changed = False 00047 00048 00049 def reset_cb(self, msg): 00050 self.reset() 00051 00052 00053 00054 00055 00056 00057 def reset(self): 00058 with self.lock: 00059 self.state = None 00060 count = len(self.meas) 00061 self.meas = [] 00062 self.timestamps = [] 00063 rospy.loginfo('Reset calibration state') 00064 print "\033[33;1m%ld\033[0m previous measurements in the cache are deleted.\n\n" % count 00065 00066 00067 def getAngle(self, quaternion): 00068 if (math.fabs(quaternion[3])>1): 00069 print "quaternion[3] out of range" # send out a notice only 00070 s = 2.* math.acos(quaternion[3]) 00071 return s 00072 00073 def getAxis(self, quaternion): 00074 s_squared = 1. - math.pow(quaternion[3], 2.) 00075 if s_squared < 10. * 1e-5 : #FLT_EPSILON 1e-5 00076 return (1.0, 0.0, 0.0) #Arbitrary 00077 s = math.sqrt(s_squared) 00078 return (quaternion[0]/s, quaternion[1]/s, quaternion[2]/s) 00079 00080 00081 00082 def meas_cb(self, msg): 00083 with self.lock: 00084 # check if cam_info is valid 00085 for camera in msg.M_cam: 00086 P = camera.cam_info.P 00087 all_zero = True 00088 for i in range(12): 00089 if P[i] != 0: 00090 all_zero = False 00091 if all_zero: 00092 rospy.logfatal("Camera info of %s is all zero. You should calibrate your camera intrinsics first "%camera.camera_id) 00093 exit(-1) 00094 00095 # Modify all the camera info messages to make sure that ROI and binning are removed, and P is updated accordingly 00096 # Update is done in place 00097 #for camera in msg.M_cam: 00098 # camera.cam_info = camera_info_converter.unbin(camera.cam_info) 00099 00100 00101 00102 if rospy.has_param('optimizer_conditional_resetter_enable'): 00103 self.reset_flag = rospy.get_param('optimizer_conditional_resetter_enable') 00104 else: 00105 self.reset_flag = False 00106 00107 00108 00109 00110 if self.reset_flag == True : 00111 urdf_cam_ns = rospy.get_param("urdf_cam_ns") 00112 new_cam_ns = rospy.get_param("new_cam_ns") 00113 u_info = rospy.wait_for_message(urdf_cam_ns+'/camera_info', CameraInfo) 00114 n_info = rospy.wait_for_message(new_cam_ns+'/camera_info', CameraInfo) 00115 urdf_cam_frame = u_info.header.frame_id 00116 new_cam_frame = n_info.header.frame_id 00117 mounting_frame = rospy.get_param("mounting_frame") 00118 00119 00120 # if any of the frames has changed... 00121 if self.prev_3_frames != () : 00122 if self.prev_3_frames != (urdf_cam_frame, new_cam_frame, mounting_frame): 00123 self.frames_changed = True 00124 00125 if self.frames_changed : 00126 self.state = None 00127 self.meas = [] # clear cache 00128 self.timestamps = [] 00129 self.measurement_count = 0 00130 self.prev_tf_transforms = {} 00131 rospy.loginfo('Reset calibration state') 00132 print "\033[43;1mTarget frames have changed. Clean up everything and start over! \033[0m\n\n" 00133 self.frames_changed = False 00134 00135 00136 if len(self.tf_check_pairs) > 0 : 00137 self.prev_tf_pair = self.tf_check_pairs[0] 00138 self.tf_check_pairs = [] 00139 self.tf_check_pairs.append((mounting_frame, urdf_cam_frame)) # only keep one pair in the list at a time 00140 00141 self.prev_3_frames = (urdf_cam_frame, new_cam_frame, mounting_frame) 00142 self.prev_tf_pair = (mounting_frame, urdf_cam_frame) 00143 00144 TheMoment = msg.header.stamp 00145 00146 # check for tf transform changes 00147 for (frame1, frame2) in self.tf_check_pairs: 00148 transform = None 00149 print "\nLooking up transformation \033[34;1mfrom\033[0m %s \033[34;1mto\033[0m %s ..." % (frame1, frame2) 00150 while not rospy.is_shutdown(): 00151 try: 00152 self.tf_listener.waitForTransform(frame1, frame2, TheMoment, rospy.Duration(10)) 00153 transform = self.tf_listener.lookupTransform(frame1, frame2, TheMoment) #((),()) 00154 print "found\n" 00155 break 00156 except (tf.LookupException, tf.ConnectivityException): 00157 print "transform lookup failed, retrying..." 00158 00159 if self.measurement_count > 0: 00160 prev_transform = self.prev_tf_transforms[(frame1, frame2)] 00161 dx = transform[0][0] - prev_transform[0][0] 00162 dy = transform[0][1] - prev_transform[0][1] 00163 dz = transform[0][2] - prev_transform[0][2] 00164 dAngle = self.getAngle(transform[1]) - self.getAngle(prev_transform[1]) 00165 a=self.getAxis(transform[1]) 00166 b=self.getAxis(prev_transform[1]) 00167 dAxis = (a[0]*b[0] + a[1]*b[1] + a[2]*b[2])/1. # a dot b 00168 00169 #print "\n" 00170 #print "\033[34;1m-- Debug Info --\033[0m" 00171 print "checking for pose change..." 00172 print "measurement_count = %d" % self.measurement_count 00173 print " dx = %10f | < 0.0002 m" % dx 00174 print " dy = %10f | < 0.0002 m" % dy 00175 print " dz = %10f | < 0.0002 m" % dz 00176 print "dAngle = %10f | < 0.00174 rad" % dAngle 00177 print " dAxis = %10f | > 0.99999\n" % dAxis 00178 00179 if ( (math.fabs(dx) > 0.0002) or (math.fabs(dy) > 0.0002) or (math.fabs(dz) > 0.0002) or 00180 (math.fabs(dAngle)>0.00174) or (math.fabs(dAxis) < 0.99999) ) : # if transform != previous_transform: 00181 print "\033[44;1mDetect pose change: %s has changed pose relative to %s since last time!\033[0m\n\n" % (frame1, frame2) 00182 ###self.reset() # no, you cannot call reset() here. 00183 00184 self.state = None 00185 count = len(self.meas) 00186 self.meas = [] # clear cache 00187 self.timestamps = [] 00188 rospy.loginfo('Reset calibration state') 00189 print "\033[33;1m%ld\033[0m previous measurements in the cache are deleted.\n\n" % count 00190 self.measurement_count = 0 00191 00192 self.prev_tf_transforms[(frame1, frame2)] = transform 00193 00194 00195 00196 00197 # add timestamp to list 00198 self.timestamps.append(msg.header.stamp) 00199 00200 00201 # add measurements to list 00202 self.meas.append(msg) 00203 print "MEAS", len(self.meas) 00204 for m in self.meas: 00205 print " - stamp: %f"%m.header.stamp.to_sec() 00206 00207 print "\n" 00208 00209 self.measurement_count += 1 00210 00211 print "\nNo. of measurements fed to optimizer: %d\n\n" % self.measurement_count 00212 00213 ## self.last_stamp_pub.publish(self.meas[-1].header.stamp) 00214 00215 # initialize state if needed 00216 if True: #not self.state: 00217 self.state = CalibrationEstimate() 00218 camera_poses, checkerboard_poses = init_optimization_prior.find_initial_poses(self.meas) 00219 self.state.targets = [ posemath.toMsg(checkerboard_poses[i]) for i in range(len(checkerboard_poses)) ] 00220 self.state.cameras = [ CameraPose(camera_id, posemath.toMsg(camera_pose)) for camera_id, camera_pose in camera_poses.iteritems()] 00221 00222 print "Proceed to optimization...\n" 00223 00224 # run optimization 00225 self.state = estimate.enhance(self.meas, self.state) 00226 00227 print "\nOptimized!\n" 00228 00229 # publish calibration state 00230 res = CameraCalibration() ## initialize a new Message instance 00231 res.camera_pose = [camera.pose for camera in self.state.cameras] 00232 res.camera_id = [camera.camera_id for camera in self.state.cameras] 00233 res.time_stamp = [timestamp for timestamp in self.timestamps] #copy 00234 res.m_count = len(res.time_stamp); # self.measurement_count 00235 self.pub.publish(res) 00236 00237 00238 def main(): 00239 rospy.init_node('online_calibration') 00240 e = Estimator() 00241 rospy.spin() 00242 00243 if __name__ == '__main__': 00244 main() 00245 00246 00247