error_visualization.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2008-2012, Willow Garage, Inc.
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of Willow Garage, Inc. nor the names of its
00018 #    contributors may be used to endorse or promote products derived
00019 #    from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 
00034 # author: Vijay Pradeep, Michael Ferguson
00035 
00036 import roslib; roslib.load_manifest('calibration_estimation')
00037 
00038 import sys
00039 import rospy
00040 import time
00041 import numpy
00042 import rosbag
00043 import yaml
00044 import os.path
00045 import numpy
00046 from numpy import kron, ones, eye, array, matrix, diag
00047 from calibration_estimation.cal_bag_helpers import *
00048 import calibration_estimation.multi_step_cov_estimator as est_helpers
00049 import calibration_estimation.opt_runner as opt_runner
00050 from calibration_estimation.sensors.multi_sensor import MultiSensor
00051 from calibration_estimation.urdf_params import UrdfParams
00052 from calibration_estimation.single_transform import SingleTransform
00053 from visualization_msgs.msg import Marker, MarkerArray
00054 import geometry_msgs.msg
00055 
00056 def usage():
00057     rospy.logerr("Not enough arguments")
00058     print "Usage:"
00059     print " ./error_visualization.py [bagfile] [output_dir] [loop_list.yaml]"
00060     sys.exit(0)
00061 
00062 if __name__ == '__main__':
00063     rospy.init_node("error_visualization")
00064     marker_pub = rospy.Publisher("calibration_error", Marker)
00065 
00066     print "Starting The Error Visualization App\n"
00067 
00068     if (len(rospy.myargv()) < 3):
00069         usage()
00070     else:
00071         bag_filename = rospy.myargv()[1]
00072         output_dir = rospy.myargv()[2]
00073         loop_list_filename = rospy.myargv()[3]
00074 
00075     print "Using Bagfile: %s\n" % bag_filename
00076     if not os.path.isfile(bag_filename):
00077         rospy.logerr("Bagfile does not exist. Exiting...")
00078         sys.exit(1)
00079 
00080     loop_list = yaml.load(open(loop_list_filename))
00081     robot_description = get_robot_description(bag_filename)
00082 
00083     config_param_name = "calibration_config"
00084     if not rospy.has_param(config_param_name):
00085         rospy.logerr("Could not find parameter [%s]. Please populate this namespace with the estimation configuration.", config_param_name)
00086         sys.exit(1)
00087     config = rospy.get_param(config_param_name)
00088 
00089     # Process all the sensor definitions that are on the parameter server
00090     sensors_name = "sensors"
00091     if sensors_name not in config.keys():
00092         rospy.logerr("Could not find namespace [%s/%s]. Please populate this namespace with sensors.", (config_param_name, sensors_name))
00093         sys.exit(1)
00094     all_sensors_dict = est_helpers.build_sensor_defs(config[sensors_name])
00095     all_sensor_types = list(set([x['sensor_type'] for x in all_sensors_dict.values()]))
00096 
00097     # Load all the calibration steps.
00098     step_list = est_helpers.load_calibration_steps(config["cal_steps"])
00099     # Only process the last step
00100     cur_step = step_list[-1]
00101 
00102     # Load the resulting system definition
00103     system_def_dict = yaml.load(open(output_dir + "/" + cur_step["output_filename"] + ".yaml"))
00104     system_def = UrdfParams(robot_description, system_def_dict)
00105     cb_poses = yaml.load(open(output_dir + "/" + cur_step["output_filename"] + "_poses.yaml"))
00106     free_dict = yaml.load(cur_step["free_params"])
00107 
00108     sample_skip_list = rospy.get_param('calibration_skip_list', [])
00109     scatter_list = []
00110     marker_count = 0
00111     label_list = list()
00112     for cur_loop in loop_list:
00113         marker_count += 1
00114 
00115         sensor_defs = est_helpers.load_requested_sensors(all_sensors_dict, [cur_loop['cam'], cur_loop['3d']])
00116         multisensors = get_multisensors(bag_filename, sensor_defs, sample_skip_list)
00117 
00118         if (len([ms for ms in multisensors if len(ms.sensors) == 2]) == 0):
00119             print "********** No Data for [%s] + [%s] pair **********" % (cur_loop['cam'], cur_loop['3d'])
00120             continue
00121         label_list.append(cur_loop)
00122 
00123         # Only grab the samples that have both the requested cam and requested 3D sensor
00124         multisensors_pruned, cb_poses_pruned = zip(*[(ms,cb) for ms,cb in zip(multisensors, cb_poses) if len(ms.sensors) == 2])
00125         sample_ind = [k for k,ms in zip(range(len(multisensors)), multisensors) if len(ms.sensors) == 2]
00126         sample_ind = [i for i in sample_ind if i not in sample_skip_list]
00127 
00128         print "Sample Indices:"
00129         print ", ".join(["%u" % i for i in sample_ind])
00130 
00131         for ms in multisensors:
00132             ms.update_config(system_def)
00133 
00134         error_calc = opt_runner.ErrorCalc(system_def, free_dict, multisensors_pruned, False)
00135         opt_all_vec = opt_runner.build_opt_vector(system_def, free_dict, numpy.array(cb_poses_pruned))
00136         e = error_calc.calculate_error(opt_all_vec)
00137 
00138         # Display error breakdown
00139         errors_dict = opt_runner.compute_errors_breakdown(error_calc, multisensors_pruned, numpy.array(cb_poses_pruned))
00140 
00141         print ""
00142         print "Errors Breakdown:"
00143         for sensor_id, error_list in errors_dict.items():
00144             print "  %s:" % sensor_id
00145             i = 0
00146             for error in error_list:
00147                 if i in sample_ind:        
00148                     rms_error = numpy.sqrt( numpy.mean(error**2) )
00149                     print "    Sample %d: %.6f" % (i, rms_error)
00150                 i += 1
00151             error_cat = numpy.concatenate(error_list)
00152             rms_error = numpy.sqrt( numpy.mean(error_cat**2) )
00153             print "    Total: %.6f" % rms_error
00154 
00155         # Calculate loop errors
00156         chain_sensors = [[s for s in ms.sensors if s.sensor_id == cur_loop['3d']][0]  for ms in multisensors_pruned]
00157         cam_sensors   = [[s for s in ms.sensors if s.sensor_id == cur_loop['cam']][0] for ms in multisensors_pruned]
00158         fk_points = [s.get_measurement() for s in chain_sensors]
00159         cb_points = [SingleTransform(pose).transform * system_def.checkerboards[ms.checkerboard].generate_points() for pose, ms in zip(cb_poses_pruned,multisensors_pruned)]
00160 
00161         points_list_fk    = [ geometry_msgs.msg.Point(cur_pt[0, 0], cur_pt[0, 1], cur_pt[0, 2]) for cur_pt in list(numpy.concatenate(fk_points,1).T)]
00162         points_list_guess = [ geometry_msgs.msg.Point(cur_pt[0, 0], cur_pt[0, 1], cur_pt[0, 2]) for cur_pt in list(numpy.concatenate(cb_points,1).T)]
00163 
00164         m = Marker()
00165         m.header.frame_id = system_def.base_link
00166         m.ns = loop_list_filename+"_points_3d"
00167         m.id = marker_count
00168         m.type = Marker.SPHERE_LIST
00169         m.action = Marker.MODIFY
00170         m.points = points_list_fk
00171         m.color.r = 0.0
00172         m.color.g = 0.0
00173         m.color.b = 1.0
00174         m.color.a = 1.0
00175         m.scale.x = 0.01
00176         m.scale.y = 0.01
00177         m.scale.z = 0.01
00178         marker_pub.publish(m)
00179 
00180         m.points = points_list_guess
00181         m.ns = loop_list_filename+"_points_cam"
00182         m.color.r = 1.0
00183         m.color.g = 0.0
00184         m.color.b = 0.0
00185         marker_pub.publish(m)
00186 
00187         cam_Js   = [s.compute_expected_J(fk) for s,fk in zip(cam_sensors, fk_points)]
00188         cam_covs = [matrix(array(s.compute_cov(fk)) * kron(eye(s.get_residual_length()/2),ones([2,2]))) for s,fk in zip(cam_sensors, fk_points)]
00189         fk_covs  = [matrix(array(s.compute_cov(None)) * kron(eye(s.get_residual_length()/3),ones([3,3]))) for s in chain_sensors]
00190 
00191         full_covs = [matrix(cam_J)*fk_cov*matrix(cam_J).T + cam_cov for cam_J, cam_cov, fk_cov in zip(cam_Js, cam_covs, fk_covs)]
00192 
00193         proj_points = [s.compute_expected(pts) for (s,pts) in zip(cam_sensors,fk_points)]
00194         meas_points = [s.get_measurement() for s in cam_sensors]
00195 
00196         r1 = numpy.concatenate(meas_points)
00197         r2 = numpy.concatenate(proj_points)
00198         r = numpy.concatenate((r1,r2),axis=0)
00199 
00200         import matplotlib.pyplot as plt
00201         
00202         cur_scatter = plt.scatter(array(r)[:,0], array(r)[:,1], **cur_loop['plot_ops'])
00203         scatter_list.append(cur_scatter)
00204         
00205     plt.gca().invert_yaxis()
00206     plt.axis('equal')
00207     plt.grid(True)
00208     plt.legend(scatter_list, [x['name'] for x in label_list], prop={'size':'x-small'})
00209     plt.show()
00210 
00211     rospy.spin()
00212     sys.exit(0)
00213 


calibration_estimation
Author(s): Vijay Pradeep, Michael Ferguson
autogenerated on Wed Aug 26 2015 10:56:21