opt_runner.py
Go to the documentation of this file.
1 # Software License Agreement (BSD License)
2 #
3 # Copyright (c) 2008, Willow Garage, Inc.
4 # All rights reserved.
5 #
6 # Redistribution and use in source and binary forms, with or without
7 # modification, are permitted provided that the following conditions
8 # are met:
9 #
10 # * Redistributions of source code must retain the above copyright
11 # notice, this list of conditions and the following disclaimer.
12 # * Redistributions in binary form must reproduce the above
13 # copyright notice, this list of conditions and the following
14 # disclaimer in the documentation and/or other materials provided
15 # with the distribution.
16 # * Neither the name of Willow Garage, Inc. nor the names of its
17 # contributors may be used to endorse or promote products derived
18 # from this software without specific prior written permission.
19 #
20 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 # POSSIBILITY OF SUCH DAMAGE.
32 
33 # author: Vijay Pradeep
34 
35 import roslib; roslib.load_manifest('calibration_estimation')
36 
37 from calibration_estimation.urdf_params import UrdfParams
38 from calibration_estimation.single_transform import SingleTransform
39 import numpy
40 from numpy import array, matrix, zeros, cumsum, concatenate, reshape
41 import scipy.optimize
42 import sys
43 import rospy
44 from visualization_msgs.msg import Marker, MarkerArray
45 import geometry_msgs.msg
46 
47 class ErrorCalc:
48  """
49  Helpers for computing errors and jacobians
50  """
51  def __init__(self, robot_params, free_dict, multisensors, use_cov):
52  self._robot_params = robot_params
53  self._expanded_params = robot_params.deflate()
54  self._free_list = robot_params.calc_free(free_dict)
55  self._multisensors = multisensors
56  self._use_cov = use_cov
57  self._j_count = 0
58  self.marker_pub = rospy.Publisher("pose_guesses", Marker)
59 
60 
61  def calculate_full_param_vec(self, opt_param_vec):
62  '''
63  Take the set of optimization params, and expand it into the bigger param vec
64  '''
65  full_param_vec = self._expanded_params.copy()
66  full_param_vec[numpy.where(self._free_list), 0] = opt_param_vec
67 
68  return full_param_vec
69 
70  def calculate_error(self, opt_all_vec):
71  opt_param_vec, full_pose_arr = self.split_all(opt_all_vec)
72 
73  full_param_vec = self.calculate_full_param_vec(opt_param_vec)
74 
75  # Update the primitives with the new set of parameters
76  self._robot_params.inflate(full_param_vec)
77 
78  # Update all the blocks' configs
79  for multisensor in self._multisensors:
80  multisensor.update_config(self._robot_params)
81 
82  r_list = []
83  id = 0
84  for multisensor, cb_pose_vec in zip(self._multisensors, list(full_pose_arr)):
85  # Process cb pose
86  cb_points = SingleTransform(cb_pose_vec).transform * self._robot_params.checkerboards[multisensor.checkerboard].generate_points()
87  if (self._use_cov):
88  r_list.append(multisensor.compute_residual_scaled(cb_points))
89  else:
90  r_list.append(multisensor.compute_residual(cb_points))
91 
92  cb_points_msgs = [ geometry_msgs.msg.Point(cur_pt[0,0], cur_pt[0,1], cur_pt[0,2]) for cur_pt in cb_points.T]
93 
94  m = Marker()
95  m.header.frame_id = self._robot_params.base_link
96  m.ns = "points_3d"
97  m.id = id
98  m.type = Marker.SPHERE_LIST
99  m.action = Marker.MODIFY
100  m.points = cb_points_msgs
101  m.color.r = 1.0
102  m.color.g = 0.0
103  m.color.b = 1.0
104  m.color.a = 1.0
105  m.scale.x = 0.01
106  m.scale.y = 0.01
107  m.scale.z = 0.01
108  self.marker_pub.publish(m)
109 
110  cur_pt = cb_points.T[0]
111 
112  m_id = Marker()
113  m_id.header.frame_id = self._robot_params.base_link
114  m_id.ns = "points_ids"
115  m_id.id = id + 10000
116  m_id.type = Marker.TEXT_VIEW_FACING
117  m_id.action = Marker.MODIFY
118  m_id.text = str(id)
119  m_id.color.r = 1.0
120  m_id.color.g = 0.0
121  m_id.color.b = 0.0
122  m_id.color.a = 1.0
123  m_id.scale.x = 0.06
124  m_id.scale.y = 0.06
125  m_id.scale.z = 0.06
126  m_id.pose.position.x = cur_pt[0,0];
127  m_id.pose.position.y = cur_pt[0,1];
128  m_id.pose.position.z = cur_pt[0,2] + 0.05;
129  m_id.pose.orientation.x = 0.0;
130  m_id.pose.orientation.y = 0.0;
131  m_id.pose.orientation.z = 0.0;
132  m_id.pose.orientation.w = 1.0;
133  self.marker_pub.publish(m_id)
134 
135  id += 1
136 
137  r_vec = concatenate(r_list)
138 
139  rms_error = numpy.sqrt( numpy.mean(r_vec**2) )
140 
141  print "\t\t\t\t\tRMS error: %.3f \r" % rms_error,
142  sys.stdout.flush()
143 
144  return array(r_vec)
145 
146  def calculate_jacobian(self, opt_all_vec):
147  """
148  Full Jacobian:
149  The resulting returned jacobian can be thought of as a set of several concatenated jacobians as follows:
150 
151  [ J_multisensor_0 ]
152  J = [ J_multisensor_1 ]
153  [ : ]
154  [ J_multisensor_M ]
155 
156  where M is the number of multisensors, which is generally the number of calibration samples collected.
157 
158  Multisensor Jacobian:
159  The Jacobian for a given multisensor is created by concatenating jacobians for each sensor
160 
161  [ J_sensor_m_0 ]
162  J_multisensor_m = [ J_sensor_m_1 ]
163  [ : ]
164  [ J_sensor_m_S ]
165 
166  where S is the number of sensors in this multisensor.
167 
168  Sensor Jacobian:
169  A single sensor jacobian is created by concatenating jacobians for the system parameters and checkerboards
170 
171  J_sensor_m_s = [ J_params_m_s | J_m_s_pose0 | J_m_s_pose1 | --- | J_m_s_CB_poseC ]
172 
173  The number of rows in J_sensor_m_s is equal to the number of rows in this sensor's residual.
174  J_params_m_s shows how modifying the free system parameters affects the residual.
175  Each J_m_s_pose_c is 6 columns wide and shows how moving a certain target affects the residual. All
176  of the J_m_s_pose blocks are zero, except J_sensor_pose_m, since target m is the only target that
177  was viewed by the sensors in this multisensor.
178  """
179  self._j_count += 1;
180  sys.stdout.write(" \r")
181  sys.stdout.write("Computing Jacobian.. (cycle #%d)\r" % self._j_count)
182  sys.stdout.flush()
183  #import scipy.optimize.slsqp.approx_jacobian as approx_jacobian
184  #J = approx_jacobian(opt_param_vec, self.calculate_error, 1e-6)
185 
186  opt_param_vec, full_pose_arr = self.split_all(opt_all_vec)
187 
188  # Allocate the full jacobian matrix
189  ms_r_len = [ms.get_residual_length() for ms in self._multisensors]
190  J = zeros([sum(ms_r_len), len(opt_all_vec)])
191 
192  # Calculate at which row each multisensor starts and ends
193  ms_end_row = list(cumsum(ms_r_len))
194  ms_start_row = [0] + ms_end_row[:-1]
195 
196  # Calculate at which column each multisensor starts and ends
197  ms_end_col = list(cumsum([6]*len(self._multisensors)) + len(opt_param_vec))
198  ms_start_col = [x-6 for x in ms_end_col]
199 
200 
201  for i,ms in enumerate(self._multisensors):
202  # Populate the parameter section for this multisensor
203  J_ms_params = J[ ms_start_row[i]:ms_end_row[i],
204  0:len(opt_param_vec) ]
205  s_r_len = [s.get_residual_length() for s in ms.sensors]
206  s_end_row = list(cumsum(s_r_len))
207  s_start_row = [0] + s_end_row[:-1]
208  target_pose_T = SingleTransform(full_pose_arr[i,:]).transform
209  # Fill in parameter section one sensor at a time
210  for k,s in enumerate(ms.sensors):
211  J_s_params = J_ms_params[ s_start_row[k]:s_end_row[k], :]
212  J_s_params[:,:] = self.single_sensor_params_jacobian(opt_param_vec, target_pose_T, ms.checkerboard, s)
213 
214  # Populate the pose section for this multisensor
215  J_ms_pose = J[ ms_start_row[i]:ms_end_row[i],
216  ms_start_col[i]:ms_end_col[i]]
217  assert(J_ms_pose.shape[1] == 6)
218  J_ms_pose[:,:] = self.multisensor_pose_jacobian(opt_param_vec, full_pose_arr[i,:], ms)
219 
220  sys.stdout.flush()
221 
222  return J
223 
224  def split_all(self, opt_all_vec):
225  """
226  Splits the input vector into two parts:
227  1) opt_param_vec: The system parameters that we're optimizing,
228  2) full_pose_arr: A Nx6 array where each row is a checkerboard pose
229  """
230  opt_param_len = sum(self._free_list)
231  opt_param_vec = opt_all_vec[0:opt_param_len]
232 
233  full_pose_vec = opt_all_vec[opt_param_len:]
234  full_pose_arr = numpy.reshape(full_pose_vec, [-1,6])
235  return opt_param_vec, full_pose_arr
236 
237  def single_sensor_params_jacobian(self, opt_param_vec, target_pose_T, target_id, sensor):
238  """
239  Computes the jacobian from the free system parameters to a single sensor
240  Inputs:
241  - opt_param_vec: Vector of the free system parameters (the parameters we're actuall optimizing)
242  - target_pose_T: The pose of the target that this sensor is viewing
243  - target_id: String name of the target that this sensor is viewing. This is necessary to generate the
244  set of feature points on the target
245  - sensor: The actual sensor definition
246  Output:
247  - J_scaled: An mxn jacobian, where m is the length of sensor's residual and n is the length of opt_param_vec.
248  If covariance calculations are enabled, then the Jacobian is scaled by sqrt(Gamma), where Gamma
249  is the information matrix for this measurement.
250  """
251  sparsity_dict = sensor.build_sparsity_dict()
252  required_keys = ['chains', 'tilting_lasers', 'transforms', 'rectified_cams', 'checkerboards']
253  for cur_key in required_keys:
254  if cur_key not in sparsity_dict.keys():
255  sparsity_dict[cur_key] = {}
256  # Generate the full sparsity vector
257  full_sparsity_list = self._robot_params.calc_free(sparsity_dict)
258  full_sparsity_vec = numpy.array(full_sparsity_list)
259 
260  # Extract the sparsity for only the parameters we are optimizing over
261 
262  opt_sparsity_vec = full_sparsity_vec[numpy.where(self._free_list)].copy()
263 
264  # Update the primitives with the new set of parameters
265  full_param_vec = self.calculate_full_param_vec(opt_param_vec)
266  self._robot_params.inflate(full_param_vec)
267  sensor.update_config(self._robot_params)
268 
269  # based on code from scipy.slsqp
270  x0 = opt_param_vec
271  epsilon = 1e-6
272  target_points = target_pose_T * self._robot_params.checkerboards[target_id].generate_points()
273  f0 = sensor.compute_residual(target_points)
274  if (self._use_cov):
275  gamma_sqrt = sensor.compute_marginal_gamma_sqrt(target_points)
276  Jt = numpy.zeros([len(x0),len(f0)])
277  dx = numpy.zeros(len(x0))
278  for i in numpy.where(opt_sparsity_vec)[0]:
279  dx[i] = epsilon
280  opt_test_param_vec = x0 + dx
281  full_test_param_vec = self.calculate_full_param_vec(opt_test_param_vec)
282  self._robot_params.inflate(full_test_param_vec)
283  sensor.update_config(self._robot_params)
284  target_points = target_pose_T * self._robot_params.checkerboards[target_id].generate_points()
285  Jt[i] = (sensor.compute_residual(target_points) - f0)/epsilon
286  dx[i] = 0.0
287  J = Jt.transpose()
288  if (self._use_cov):
289  J_scaled = gamma_sqrt * J
290  else:
291  J_scaled = J
292  return J_scaled
293 
294  def multisensor_pose_jacobian(self, opt_param_vec, pose_param_vec, multisensor):
295  """
296  Generates the jacobian from a target pose to the multisensor's residual.
297 
298  Input:
299  - opt_param_vec: Vector of the free system parameters (the parameters we're actuall optimizing)
300  - pose_param_vec: Vector of length 6 encoding the target's pose 0:3=translation 3:6=rotation_axis
301  - multisensor: The actual multisensor definition.
302  Output:
303  - J_scaled: An mx6 jacobian, where m is the length of multisensor's residual. There are 6 columns since the
304  target's pose is encoded using 6 parameters.
305  If covariance calculations are enabled, then the Jacobian is scaled by sqrt(Gamma), where Gamma
306  is the information matrix for this measurement.
307  """
308  # Update the primitives with the new set of parameters
309  full_param_vec = self.calculate_full_param_vec(opt_param_vec)
310  self._robot_params.inflate(full_param_vec)
311  multisensor.update_config(self._robot_params)
312  cb_model = self._robot_params.checkerboards[multisensor.checkerboard]
313  local_cb_points = cb_model.generate_points()
314 
315  # based on code from scipy.slsqp
316  x0 = pose_param_vec
317  epsilon = 1e-6
318  world_pts = SingleTransform(x0).transform * local_cb_points
319  f0 = multisensor.compute_residual(world_pts)
320  if (self._use_cov):
321  gamma_sqrt = multisensor.compute_marginal_gamma_sqrt(world_pts)
322 
323  Jt = numpy.zeros([len(x0),len(f0)])
324  dx = numpy.zeros(len(x0))
325  for i in range(len(x0)):
326  dx[i] = epsilon
327  test_vec = x0 + dx
328  fTest = multisensor.compute_residual(SingleTransform(test_vec).transform * local_cb_points)
329  Jt[i] = (fTest - f0)/epsilon
330  #import code; code.interact(local=locals())
331  dx[i] = 0.0
332  J = Jt.transpose()
333  if (self._use_cov):
334  J_scaled = gamma_sqrt * J
335  else:
336  J_scaled = J
337  return J_scaled
338 
339 def build_opt_vector(robot_params, free_dict, pose_guess_arr):
340  """
341  Construct vector of all the parameters that we're optimizing over. This includes
342  both the free system parameters and the checkerboard poses
343  Inputs:
344  - robot_params: Dictionary with all of the system parameters
345  - free_dict: Dictionary that specifies which system parameters are free
346  - pose_guess_arr: Mx6 array storing the current checkerboard poses, where M is
347  the number of calibration samples
348  Returns: Vector of length (F + Mx6), where F is the number of 1's in the free_dict
349  """
350 
351  # Convert the robot parameter dictionary into a vector
352  expanded_param_vec = robot_params.deflate()
353  free_list = robot_params.calc_free(free_dict)
354 
355  # Pull out only the free system parameters from the system parameter vector
356  opt_param_vec = array(expanded_param_vec[numpy.where(free_list)].T)[0]
357 
358  assert(pose_guess_arr.shape[1] == 6)
359  opt_pose_vec = reshape(pose_guess_arr, [-1])
360 
361  return numpy.concatenate([opt_param_vec, opt_pose_vec])
362 
363 def compute_errors_breakdown(error_calc, multisensors, opt_pose_arr):
364  errors_dict = {}
365  # Compute the error for each sensor type
366  for ms, k in zip(multisensors, range(len(multisensors))):
367  cb = error_calc._robot_params.checkerboards[ms.checkerboard]
368  cb_pose_vec = opt_pose_arr[k]
369  target_pts = SingleTransform(cb_pose_vec).transform * cb.generate_points()
370  for sensor in ms.sensors:
371  r_sensor = sensor.compute_residual(target_pts) * numpy.sqrt(sensor.terms_per_sample) # terms per sample is a hack to find rms distance, instead of a pure rms, based on each term
372  if sensor.sensor_id not in errors_dict.keys():
373  errors_dict[sensor.sensor_id] = []
374  errors_dict[sensor.sensor_id].append(r_sensor)
375  return errors_dict
376 
377 def opt_runner(robot_params, pose_guess_arr, free_dict, multisensors, use_cov):
378  """
379  Runs a single optimization step for the calibration optimization.
380  robot_params - Instance of UrdfParams
381  free_dict - Dictionary storing which parameters are free
382  multisensor - list of list of measurements. Each multisensor corresponds to a single checkerboard pose
383  pose_guesses - List of guesses as to where all the checkerboard are. This is used to initialze the optimization
384  """
385  error_calc = ErrorCalc(robot_params, free_dict, multisensors, use_cov)
386 
387  # Construct the initial guess
388  opt_all = build_opt_vector(robot_params, free_dict, pose_guess_arr)
389 
390  x, cov_x, infodict, mesg, iter = scipy.optimize.leastsq(error_calc.calculate_error, opt_all, Dfun=error_calc.calculate_jacobian, full_output=1)
391 
392  J = error_calc.calculate_jacobian(x)
393 
394  # A hacky way to inflate x back into robot params
395  opt_param_vec, pose_vec = error_calc.split_all(x)
396  expanded_param_vec = error_calc.calculate_full_param_vec(opt_param_vec)
397  opt_pose_arr = reshape(pose_vec, [-1, 6])
398 
399  output_dict = error_calc._robot_params.params_to_config(expanded_param_vec)
400 
401  errors_dict = compute_errors_breakdown(error_calc, multisensors, opt_pose_arr)
402 
403  print ""
404  print "Errors Breakdown:"
405  for sensor_id, error_list in errors_dict.items():
406  error_cat = numpy.concatenate(error_list)
407  rms_error = numpy.sqrt( numpy.mean(error_cat**2) )
408  print " %s: %.6f" % (sensor_id, rms_error)
409 
410  return output_dict, opt_pose_arr, J
411 
def single_sensor_params_jacobian(self, opt_param_vec, target_pose_T, target_id, sensor)
Definition: opt_runner.py:237
def calculate_error(self, opt_all_vec)
Definition: opt_runner.py:70
def multisensor_pose_jacobian(self, opt_param_vec, pose_param_vec, multisensor)
Definition: opt_runner.py:294
def build_opt_vector(robot_params, free_dict, pose_guess_arr)
Definition: opt_runner.py:339
def __init__(self, robot_params, free_dict, multisensors, use_cov)
Definition: opt_runner.py:51
def calculate_full_param_vec(self, opt_param_vec)
Definition: opt_runner.py:61
def calculate_jacobian(self, opt_all_vec)
Definition: opt_runner.py:146
def opt_runner(robot_params, pose_guess_arr, free_dict, multisensors, use_cov)
Definition: opt_runner.py:377
def compute_errors_breakdown(error_calc, multisensors, opt_pose_arr)
Definition: opt_runner.py:363


calibration_estimation
Author(s): Vijay Pradeep, Michael Ferguson
autogenerated on Thu Jun 6 2019 19:17:16