error_visualization.py
Go to the documentation of this file.
1 #! /usr/bin/env python
2 # Software License Agreement (BSD License)
3 #
4 # Copyright (c) 2008-2012, Willow Garage, Inc.
5 # All rights reserved.
6 #
7 # Redistribution and use in source and binary forms, with or without
8 # modification, are permitted provided that the following conditions
9 # are met:
10 #
11 # * Redistributions of source code must retain the above copyright
12 # notice, this list of conditions and the following disclaimer.
13 # * Redistributions in binary form must reproduce the above
14 # copyright notice, this list of conditions and the following
15 # disclaimer in the documentation and/or other materials provided
16 # with the distribution.
17 # * Neither the name of Willow Garage, Inc. nor the names of its
18 # contributors may be used to endorse or promote products derived
19 # from this software without specific prior written permission.
20 #
21 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 # POSSIBILITY OF SUCH DAMAGE.
33 
34 # author: Vijay Pradeep, Michael Ferguson
35 
36 import roslib; roslib.load_manifest('calibration_estimation')
37 
38 import sys
39 import rospy
40 import time
41 import numpy
42 import rosbag
43 import yaml
44 import os.path
45 import numpy
46 from numpy import kron, ones, eye, array, matrix, diag
49 import calibration_estimation.opt_runner as opt_runner
50 from calibration_estimation.sensors.multi_sensor import MultiSensor
51 from calibration_estimation.urdf_params import UrdfParams
52 from calibration_estimation.single_transform import SingleTransform
53 from visualization_msgs.msg import Marker, MarkerArray
54 import geometry_msgs.msg
55 
56 def usage():
57  rospy.logerr("Not enough arguments")
58  print "Usage:"
59  print " ./error_visualization.py [bagfile] [output_dir] [loop_list.yaml]"
60  sys.exit(0)
61 
62 if __name__ == '__main__':
63  rospy.init_node("error_visualization")
64  marker_pub = rospy.Publisher("calibration_error", Marker)
65 
66  print "Starting The Error Visualization App\n"
67 
68  if (len(rospy.myargv()) < 3):
69  usage()
70  else:
71  bag_filename = rospy.myargv()[1]
72  output_dir = rospy.myargv()[2]
73  loop_list_filename = rospy.myargv()[3]
74 
75  print "Using Bagfile: %s\n" % bag_filename
76  if not os.path.isfile(bag_filename):
77  rospy.logerr("Bagfile does not exist. Exiting...")
78  sys.exit(1)
79 
80  loop_list = yaml.load(open(loop_list_filename))
81  robot_description = get_robot_description(bag_filename)
82 
83  config_param_name = "calibration_config"
84  if not rospy.has_param(config_param_name):
85  rospy.logerr("Could not find parameter [%s]. Please populate this namespace with the estimation configuration.", config_param_name)
86  sys.exit(1)
87  config = rospy.get_param(config_param_name)
88 
89  # Process all the sensor definitions that are on the parameter server
90  sensors_name = "sensors"
91  if sensors_name not in config.keys():
92  rospy.logerr("Could not find namespace [%s/%s]. Please populate this namespace with sensors.", (config_param_name, sensors_name))
93  sys.exit(1)
94  all_sensors_dict = est_helpers.build_sensor_defs(config[sensors_name])
95  all_sensor_types = list(set([x['sensor_type'] for x in all_sensors_dict.values()]))
96 
97  # Load all the calibration steps.
98  step_list = est_helpers.load_calibration_steps(config["cal_steps"])
99  # Only process the last step
100  cur_step = step_list[-1]
101 
102  # Load the resulting system definition
103  system_def_dict = yaml.load(open(output_dir + "/" + cur_step["output_filename"] + ".yaml"))
104  system_def = UrdfParams(robot_description, system_def_dict)
105  cb_poses = yaml.load(open(output_dir + "/" + cur_step["output_filename"] + "_poses.yaml"))
106  free_dict = yaml.load(cur_step["free_params"])
107 
108  sample_skip_list = rospy.get_param('calibration_skip_list', [])
109  scatter_list = []
110  marker_count = 0
111  label_list = list()
112  for cur_loop in loop_list:
113  marker_count += 1
114 
115  sensor_defs = est_helpers.load_requested_sensors(all_sensors_dict, [cur_loop['cam'], cur_loop['3d']])
116  multisensors = get_multisensors(bag_filename, sensor_defs, sample_skip_list)
117 
118  if (len([ms for ms in multisensors if len(ms.sensors) == 2]) == 0):
119  print "********** No Data for [%s] + [%s] pair **********" % (cur_loop['cam'], cur_loop['3d'])
120  continue
121  label_list.append(cur_loop)
122 
123  # Only grab the samples that have both the requested cam and requested 3D sensor
124  multisensors_pruned, cb_poses_pruned = zip(*[(ms,cb) for ms,cb in zip(multisensors, cb_poses) if len(ms.sensors) == 2])
125  sample_ind = [k for k,ms in zip(range(len(multisensors)), multisensors) if len(ms.sensors) == 2]
126  sample_ind = [i for i in sample_ind if i not in sample_skip_list]
127 
128  print "Sample Indices:"
129  print ", ".join(["%u" % i for i in sample_ind])
130 
131  for ms in multisensors:
132  ms.update_config(system_def)
133 
134  error_calc = opt_runner.ErrorCalc(system_def, free_dict, multisensors_pruned, False)
135  opt_all_vec = opt_runner.build_opt_vector(system_def, free_dict, numpy.array(cb_poses_pruned))
136  e = error_calc.calculate_error(opt_all_vec)
137 
138  # Display error breakdown
139  errors_dict = opt_runner.compute_errors_breakdown(error_calc, multisensors_pruned, numpy.array(cb_poses_pruned))
140 
141  print ""
142  print "Errors Breakdown:"
143  for sensor_id, error_list in errors_dict.items():
144  print " %s:" % sensor_id
145  i = 0
146  for error in error_list:
147  if i in sample_ind:
148  rms_error = numpy.sqrt( numpy.mean(error**2) )
149  print " Sample %d: %.6f" % (i, rms_error)
150  i += 1
151  error_cat = numpy.concatenate(error_list)
152  rms_error = numpy.sqrt( numpy.mean(error_cat**2) )
153  print " Total: %.6f" % rms_error
154 
155  # Calculate loop errors
156  chain_sensors = [[s for s in ms.sensors if s.sensor_id == cur_loop['3d']][0] for ms in multisensors_pruned]
157  cam_sensors = [[s for s in ms.sensors if s.sensor_id == cur_loop['cam']][0] for ms in multisensors_pruned]
158  fk_points = [s.get_measurement() for s in chain_sensors]
159  cb_points = [SingleTransform(pose).transform * system_def.checkerboards[ms.checkerboard].generate_points() for pose, ms in zip(cb_poses_pruned,multisensors_pruned)]
160 
161  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)]
162  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)]
163 
164  m = Marker()
165  m.header.frame_id = system_def.base_link
166  m.ns = loop_list_filename+"_points_3d"
167  m.id = marker_count
168  m.type = Marker.SPHERE_LIST
169  m.action = Marker.MODIFY
170  m.points = points_list_fk
171  m.color.r = 0.0
172  m.color.g = 0.0
173  m.color.b = 1.0
174  m.color.a = 1.0
175  m.scale.x = 0.01
176  m.scale.y = 0.01
177  m.scale.z = 0.01
178  marker_pub.publish(m)
179 
180  m.points = points_list_guess
181  m.ns = loop_list_filename+"_points_cam"
182  m.color.r = 1.0
183  m.color.g = 0.0
184  m.color.b = 0.0
185  marker_pub.publish(m)
186 
187  cam_Js = [s.compute_expected_J(fk) for s,fk in zip(cam_sensors, fk_points)]
188  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)]
189  fk_covs = [matrix(array(s.compute_cov(None)) * kron(eye(s.get_residual_length()/3),ones([3,3]))) for s in chain_sensors]
190 
191  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)]
192 
193  proj_points = [s.compute_expected(pts) for (s,pts) in zip(cam_sensors,fk_points)]
194  meas_points = [s.get_measurement() for s in cam_sensors]
195 
196  r1 = numpy.concatenate(meas_points)
197  r2 = numpy.concatenate(proj_points)
198  r = numpy.concatenate((r1,r2),axis=0)
199 
200  import matplotlib.pyplot as plt
201 
202  cur_scatter = plt.scatter(array(r)[:,0], array(r)[:,1], **cur_loop['plot_ops'])
203  scatter_list.append(cur_scatter)
204 
205  plt.gca().invert_yaxis()
206  plt.axis('equal')
207  plt.grid(True)
208  plt.legend(scatter_list, [x['name'] for x in label_list], prop={'size':'x-small'})
209  plt.show()
210 
211  rospy.spin()
212  sys.exit(0)
213 
def get_robot_description(bag_filename, use_topic=False)
def get_multisensors(bag_filename, cur_sensors, sample_skip_list=[])


calibration_estimation
Author(s): Vijay Pradeep, Michael Ferguson
autogenerated on Fri Apr 2 2021 02:12:53