Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 import roslib; roslib.load_manifest('pr2_calibration_launch')
00038 import sensor_msgs.msg
00039 import sensor_msgs.srv
00040 import rospy
00041 import rosbag
00042
00043 import math
00044 import sys
00045 import yaml
00046
00047 from calibration_estimation.cal_bag_helpers import *
00048
00049 def usage():
00050 print "Usage: ./baseline_updater camera:=[camera_namespace] [calibration_bagfile] [system_filename] [config_camera_name]"
00051 print " camera_namespace: Ros name to prepend onto the call to set_calibration"
00052 print " config_filename: yaml file that stores the baseline shift"
00053 print " config_camera_name: Name of the camera in the configuration file. Tells script which baseline shift to use"
00054
00055 def main():
00056 rospy.init_node('baseline_updater', anonymous=True)
00057
00058 argv = rospy.myargv()
00059
00060 if len(argv) != 4:
00061 usage()
00062 sys.exit(0)
00063
00064 bag_filename = argv[1]
00065 system_filename = argv[2]
00066 config_cam_name = argv[3]
00067
00068 cam_info = get_cam_info(bag_filename, config_cam_name)
00069 if cam_info == None:
00070 print "Could not find a camera of the name [%s]" % config_cam_name
00071 sys.exit(-1)
00072
00073 print "Original Projection Matrix:"
00074 for k in range(3):
00075 print " %s" % " ".join(["% 12.5f" % p for p in cam_info.P[k*4:k*4+4]])
00076 original_baseline = cam_info.P[3]
00077
00078
00079 system_dict = yaml.load(open(system_filename))
00080 cam_dict = system_dict['sensors']['rectified_cams']
00081 target_cam_dict = cam_dict[config_cam_name]
00082 baseline_shift = target_cam_dict['baseline_shift']
00083
00084 print "Baseline Shift:"
00085 print " % 12.5f" % baseline_shift
00086
00087
00088 updated_baseline = original_baseline + baseline_shift
00089
00090
00091 updated_P = list(cam_info.P)
00092 updated_P[3] = updated_baseline
00093 cam_info.P = updated_P
00094
00095 print "Updated Projection Matrix:"
00096 for k in range(3):
00097 print " %s" % " ".join(["% 12.5f" % p for p in cam_info.P[k*4:k*4+4]])
00098
00099
00100 cam_name = rospy.resolve_name("camera")
00101 set_cal_service_name = cam_name + "/set_camera_info"
00102
00103 print "Waiting for service [%s] to be available" % set_cal_service_name
00104 rospy.wait_for_service(set_cal_service_name)
00105 print "Writing camera info to camera memory..."
00106 set_cal_srv = rospy.ServiceProxy(set_cal_service_name, sensor_msgs.srv.SetCameraInfo)
00107 resp = set_cal_srv(cam_info)
00108 if resp.success:
00109 print "Done writing to camera"
00110 else:
00111 print "**************** ERROR WRITING TO CAMERA. PLEASE RETRY ***********************"
00112 sys.exit(-1)
00113
00114 if __name__ == "__main__":
00115 main()