run_optimization_online.py
Go to the documentation of this file.
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 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


camera_pose_calibration
Author(s): Vijay Pradeep, Wim Meeussen
autogenerated on Thu Aug 15 2013 10:18:08