Go to the documentation of this file.00001
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"
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 :
00076 return (1.0, 0.0, 0.0)
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
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
00096
00097
00098
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
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 = []
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))
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
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.
00168
00169
00170
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) ) :
00181 print "\033[44;1mDetect pose change: %s has changed pose relative to %s since last time!\033[0m\n\n" % (frame1, frame2)
00182
00183
00184 self.state = None
00185 count = len(self.meas)
00186 self.meas = []
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
00198 self.timestamps.append(msg.header.stamp)
00199
00200
00201
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
00214
00215
00216 if True:
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
00225 self.state = estimate.enhance(self.meas, self.state)
00226
00227 print "\nOptimized!\n"
00228
00229
00230 res = CameraCalibration()
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]
00234 res.m_count = len(res.time_stamp);
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