run_optimization_prior.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 
00003 import sys, time, optparse
00004 import itertools
00005 import collections
00006 
00007 import roslib
00008 roslib.load_manifest('camera_pose_calibration')
00009 import PyKDL
00010 from tf_conversions import posemath
00011 from calibration_msgs.msg import *
00012 
00013 from camera_pose_calibration.msg import CalibrationEstimate
00014 from camera_pose_calibration.msg import CameraPose
00015 
00016 import rosbag
00017 from camera_pose_calibration import init_optimization_prior
00018 from camera_pose_calibration import estimate
00019 from camera_pose_calibration import dump_estimate
00020 
00021 import yaml
00022 import sys
00023 
00024 # read data from bag
00025 if len(sys.argv) >= 2:
00026     BAG = sys.argv[1]
00027 else:
00028     BAG = '/u/vpradeep/kinect_bags/kinect_extrinsics_2011-04-05-16-01-28.bag'
00029 bag = rosbag.Bag(BAG)
00030 for topic, msg, t in bag:
00031     assert topic == 'robot_measurement'
00032 cal_samples = [msg for topic, msg, t in bag]
00033 
00034 
00035 # create prior
00036 camera_poses, checkerboard_poses = init_optimization_prior.find_initial_poses(cal_samples)
00037 cal_estimate = CalibrationEstimate()
00038 cal_estimate.targets = [ posemath.toMsg(checkerboard_poses[i]) for i in range(len(checkerboard_poses)) ]
00039 cal_estimate.cameras = [ CameraPose(camera_id, posemath.toMsg(camera_pose)) for camera_id, camera_pose in camera_poses.iteritems()]
00040 
00041 
00042 # Run optimization
00043 new_cal_estimate = estimate.enhance(cal_samples, cal_estimate)
00044 cam_dict_list = dump_estimate.to_dict_list(new_cal_estimate.cameras)
00045 
00046 # For now, hardcode what transforms we care about
00047 tf_config = dict();
00048 tf_config['camera_a'] = {'calibrated_frame':'camera_a/openni_rgb_optical_frame',
00049                          'parent_frame': 'world_frame',
00050                          'child_frame': 'camera_a/openni_camera'}
00051 tf_config['camera_b'] = {'calibrated_frame':'camera_b/openni_rgb_optical_frame',
00052                          'parent_frame': 'world_frame',
00053                          'child_frame': 'camera_b/openni_camera'}
00054 
00055 # Insert TF Data into output file
00056 for cam_dict in cam_dict_list:
00057     cam_id = cam_dict['camera_id']
00058     if cam_id in tf_config:
00059         cam_dict['tf'] = tf_config[cam_id]
00060     else:
00061         cam_dict['tf'] = {'calibrated_frame': cam_id,
00062                           'parent_frame': 'world_frame',
00063                           'child_frame': cam_id}
00064 
00065 f = open('camera_cal.yaml', 'w')
00066 f.write(yaml.dump(cam_dict_list))
00067 f.close()
 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