propagate_baseline.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 
00003 # Software License Agreement (BSD License)
00004 #
00005 # Copyright (c) 2008, Willow Garage, Inc.
00006 # All rights reserved.
00007 #
00008 # Redistribution and use in source and binary forms, with or without
00009 # modification, are permitted provided that the following conditions
00010 # are met:
00011 #
00012 #  * Redistributions of source code must retain the above copyright
00013 #    notice, this list of conditions and the following disclaimer.
00014 #  * Redistributions in binary form must reproduce the above
00015 #    copyright notice, this list of conditions and the following
00016 #    disclaimer in the documentation and/or other materials provided
00017 #    with the distribution.
00018 #  * Neither the name of Willow Garage, Inc. nor the names of its
00019 #    contributors may be used to endorse or promote products derived
00020 #    from this software without specific prior written permission.
00021 #
00022 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 # POSSIBILITY OF SUCH DAMAGE.
00034 
00035 # author: Vijay Pradeep
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     # ****** Get the baseline shift from the yaml ******
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     # ****** Update the baseline ******
00088     updated_baseline = original_baseline + baseline_shift
00089 
00090     #import code; code.interact(local=locals())
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     # ****** Load new camera info onto camera eeprom ******
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()


pr2_calibration_launch
Author(s): Vijay Pradeep
autogenerated on Fri Aug 28 2015 12:06:40