opt_runner.py
Go to the documentation of this file.
00001 # Software License Agreement (BSD License)
00002 #
00003 # Copyright (c) 2008, Willow Garage, Inc.
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions
00008 # are met:
00009 #
00010 #  * Redistributions of source code must retain the above copyright
00011 #    notice, this list of conditions and the following disclaimer.
00012 #  * Redistributions in binary form must reproduce the above
00013 #    copyright notice, this list of conditions and the following
00014 #    disclaimer in the documentation and/or other materials provided
00015 #    with the distribution.
00016 #  * Neither the name of Willow Garage, Inc. nor the names of its
00017 #    contributors may be used to endorse or promote products derived
00018 #    from this software without specific prior written permission.
00019 #
00020 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 # POSSIBILITY OF SUCH DAMAGE.
00032 
00033 # author: Vijay Pradeep
00034 
00035 import roslib; roslib.load_manifest('calibration_estimation')
00036 
00037 from calibration_estimation.urdf_params import UrdfParams
00038 from calibration_estimation.single_transform import SingleTransform
00039 import numpy
00040 from numpy import array, matrix, zeros, cumsum, concatenate, reshape
00041 import scipy.optimize
00042 import sys
00043 
00044 class ErrorCalc:
00045     """
00046     Helpers for computing errors and jacobians
00047     """
00048     def __init__(self, robot_params, free_dict, multisensors, use_cov):
00049         self._robot_params = robot_params
00050         self._expanded_params = robot_params.deflate()
00051         self._free_list = robot_params.calc_free(free_dict)
00052         self._multisensors = multisensors
00053         self._use_cov = use_cov
00054         self._j_count = 0
00055 
00056 
00057     def calculate_full_param_vec(self, opt_param_vec):
00058         '''
00059         Take the set of optimization params, and expand it into the bigger param vec
00060         '''
00061         full_param_vec = self._expanded_params.copy()
00062         full_param_vec[numpy.where(self._free_list), 0] = opt_param_vec
00063 
00064         return full_param_vec
00065 
00066     def calculate_error(self, opt_all_vec):
00067         opt_param_vec, full_pose_arr = self.split_all(opt_all_vec)
00068 
00069         full_param_vec = self.calculate_full_param_vec(opt_param_vec)
00070 
00071         # Update the primitives with the new set of parameters
00072         self._robot_params.inflate(full_param_vec)
00073 
00074         # Update all the blocks' configs
00075         for multisensor in self._multisensors:
00076             multisensor.update_config(self._robot_params)
00077 
00078         r_list = []
00079         for multisensor, cb_pose_vec in zip(self._multisensors, list(full_pose_arr)):
00080             # Process cb pose
00081             cb_points = SingleTransform(cb_pose_vec).transform * self._robot_params.checkerboards[multisensor.checkerboard].generate_points()
00082             if (self._use_cov):
00083                 r_list.append(multisensor.compute_residual_scaled(cb_points))
00084             else:
00085                 r_list.append(multisensor.compute_residual(cb_points))
00086         r_vec = concatenate(r_list)
00087 
00088         rms_error = numpy.sqrt( numpy.mean(r_vec**2) )
00089         
00090         print "\t\t\t\t\tRMS error: %.3f    \r" % rms_error,
00091         sys.stdout.flush()
00092 
00093         return array(r_vec)
00094 
00095     def calculate_jacobian(self, opt_all_vec):
00096         """
00097         Full Jacobian:
00098           The resulting returned jacobian can be thought of as a set of several concatenated jacobians as follows:
00099 
00100                 [ J_multisensor_0 ]
00101             J = [ J_multisensor_1 ]
00102                 [        :        ]
00103                 [ J_multisensor_M ]
00104 
00105           where M is the number of multisensors, which is generally the number of calibration samples collected.
00106 
00107         Multisensor Jacobian:
00108           The Jacobian for a given multisensor is created by concatenating jacobians for each sensor
00109 
00110                             [ J_sensor_m_0 ]
00111           J_multisensor_m = [ J_sensor_m_1 ]
00112                             [      :       ]
00113                             [ J_sensor_m_S ]
00114 
00115           where S is the number of sensors in this multisensor.
00116 
00117         Sensor Jacobian:
00118           A single sensor jacobian is created by concatenating jacobians for the system parameters and checkerboards
00119 
00120           J_sensor_m_s = [ J_params_m_s | J_m_s_pose0 | J_m_s_pose1 | --- | J_m_s_CB_poseC ]
00121 
00122           The number of rows in J_sensor_m_s is equal to the number of rows in this sensor's residual.
00123           J_params_m_s shows how modifying the free system parameters affects the residual.
00124           Each J_m_s_pose_c is 6 columns wide and shows how moving a certain target affects the residual. All
00125             of the J_m_s_pose blocks are zero, except J_sensor_pose_m, since target m is the only target that
00126             was viewed by the sensors in this multisensor.
00127         """
00128         self._j_count += 1;
00129         sys.stdout.write("                     \r")
00130         sys.stdout.write("Computing Jacobian.. (cycle #%d)\r" % self._j_count)
00131         sys.stdout.flush()
00132         #import scipy.optimize.slsqp.approx_jacobian as approx_jacobian
00133         #J = approx_jacobian(opt_param_vec, self.calculate_error, 1e-6)
00134 
00135         opt_param_vec, full_pose_arr = self.split_all(opt_all_vec)
00136 
00137         # Allocate the full jacobian matrix
00138         ms_r_len = [ms.get_residual_length() for ms in self._multisensors]
00139         J = zeros([sum(ms_r_len), len(opt_all_vec)])
00140 
00141         # Calculate at which row each multisensor starts and ends
00142         ms_end_row = list(cumsum(ms_r_len))
00143         ms_start_row = [0] + ms_end_row[:-1]
00144 
00145         # Calculate at which column each multisensor starts and ends
00146         ms_end_col = list(cumsum([6]*len(self._multisensors)) + len(opt_param_vec))
00147         ms_start_col = [x-6 for x in ms_end_col]
00148 
00149 
00150         for i,ms in enumerate(self._multisensors):
00151             # Populate the parameter section for this multisensor
00152             J_ms_params = J[ ms_start_row[i]:ms_end_row[i],
00153                              0:len(opt_param_vec) ]
00154             s_r_len = [s.get_residual_length() for s in ms.sensors]
00155             s_end_row = list(cumsum(s_r_len))
00156             s_start_row = [0] + s_end_row[:-1]
00157             target_pose_T = SingleTransform(full_pose_arr[i,:]).transform
00158             # Fill in parameter section one sensor at a time
00159             for k,s in enumerate(ms.sensors):
00160                 J_s_params = J_ms_params[ s_start_row[k]:s_end_row[k], :]
00161                 J_s_params[:,:] = self.single_sensor_params_jacobian(opt_param_vec, target_pose_T, ms.checkerboard, s)
00162 
00163             # Populate the pose section for this multisensor
00164             J_ms_pose = J[ ms_start_row[i]:ms_end_row[i],
00165                            ms_start_col[i]:ms_end_col[i]]
00166             assert(J_ms_pose.shape[1] == 6)
00167             J_ms_pose[:,:] = self.multisensor_pose_jacobian(opt_param_vec, full_pose_arr[i,:], ms)
00168 
00169         sys.stdout.flush()
00170 
00171         return J
00172 
00173     def split_all(self, opt_all_vec):
00174         """
00175         Splits the input vector into two parts:
00176           1) opt_param_vec: The system parameters that we're optimizing,
00177           2) full_pose_arr: A Nx6 array where each row is a checkerboard pose
00178         """
00179         opt_param_len = sum(self._free_list)
00180         opt_param_vec = opt_all_vec[0:opt_param_len]
00181 
00182         full_pose_vec  = opt_all_vec[opt_param_len:]
00183         full_pose_arr = numpy.reshape(full_pose_vec, [-1,6])
00184         return opt_param_vec, full_pose_arr
00185 
00186     def single_sensor_params_jacobian(self, opt_param_vec, target_pose_T, target_id, sensor):
00187         """
00188         Computes the jacobian from the free system parameters to a single sensor
00189         Inputs:
00190         - opt_param_vec: Vector of the free system parameters (the parameters we're actuall optimizing)
00191         - target_pose_T: The pose of the target that this sensor is viewing
00192         - target_id: String name of the target that this sensor is viewing. This is necessary to generate the
00193                      set of feature points on the target
00194         - sensor: The actual sensor definition
00195         Output:
00196         - J_scaled: An mxn jacobian, where m is the length of sensor's residual and n is the length of opt_param_vec.
00197                     If covariance calculations are enabled, then the Jacobian is scaled by sqrt(Gamma), where Gamma
00198                     is the information matrix for this measurement.
00199         """
00200         sparsity_dict = sensor.build_sparsity_dict()
00201         required_keys = ['chains', 'tilting_lasers', 'transforms', 'rectified_cams', 'checkerboards']
00202         for cur_key in required_keys:
00203             if cur_key not in sparsity_dict.keys():
00204                 sparsity_dict[cur_key] = {}
00205         # Generate the full sparsity vector
00206         full_sparsity_list = self._robot_params.calc_free(sparsity_dict)
00207         full_sparsity_vec = numpy.array(full_sparsity_list)
00208 
00209         # Extract the sparsity for only the parameters we are optimizing over
00210 
00211         opt_sparsity_vec = full_sparsity_vec[numpy.where(self._free_list)].copy()
00212 
00213         # Update the primitives with the new set of parameters
00214         full_param_vec = self.calculate_full_param_vec(opt_param_vec)
00215         self._robot_params.inflate(full_param_vec)
00216         sensor.update_config(self._robot_params)
00217 
00218         # based on code from scipy.slsqp
00219         x0 = opt_param_vec
00220         epsilon = 1e-6
00221         target_points = target_pose_T * self._robot_params.checkerboards[target_id].generate_points()
00222         f0 = sensor.compute_residual(target_points)
00223         if (self._use_cov):
00224             gamma_sqrt = sensor.compute_marginal_gamma_sqrt(target_points)
00225         Jt = numpy.zeros([len(x0),len(f0)])
00226         dx = numpy.zeros(len(x0))
00227         for i in numpy.where(opt_sparsity_vec)[0]:
00228             dx[i] = epsilon
00229             opt_test_param_vec = x0 + dx
00230             full_test_param_vec = self.calculate_full_param_vec(opt_test_param_vec)
00231             self._robot_params.inflate(full_test_param_vec)
00232             sensor.update_config(self._robot_params)
00233             target_points = target_pose_T * self._robot_params.checkerboards[target_id].generate_points()
00234             Jt[i] = (sensor.compute_residual(target_points) - f0)/epsilon
00235             dx[i] = 0.0
00236         J = Jt.transpose()
00237         if (self._use_cov):
00238             J_scaled = gamma_sqrt * J
00239         else:
00240             J_scaled = J
00241         return J_scaled
00242 
00243     def multisensor_pose_jacobian(self, opt_param_vec, pose_param_vec, multisensor):
00244         """
00245         Generates the jacobian from a target pose to the multisensor's residual.
00246 
00247         Input:
00248         - opt_param_vec: Vector of the free system parameters (the parameters we're actuall optimizing)
00249         - pose_param_vec: Vector of length 6 encoding the target's pose 0:3=translation 3:6=rotation_axis
00250         - multisensor: The actual multisensor definition.
00251         Output:
00252         - J_scaled: An mx6 jacobian, where m is the length of multisensor's residual. There are 6 columns since the
00253                     target's pose is encoded using 6 parameters.
00254                     If covariance calculations are enabled, then the Jacobian is scaled by sqrt(Gamma), where Gamma
00255                     is the information matrix for this measurement.
00256         """
00257         # Update the primitives with the new set of parameters
00258         full_param_vec = self.calculate_full_param_vec(opt_param_vec)
00259         self._robot_params.inflate(full_param_vec)
00260         multisensor.update_config(self._robot_params)
00261         cb_model = self._robot_params.checkerboards[multisensor.checkerboard]
00262         local_cb_points = cb_model.generate_points()
00263 
00264         # based on code from scipy.slsqp
00265         x0 = pose_param_vec
00266         epsilon = 1e-6
00267         world_pts = SingleTransform(x0).transform * local_cb_points
00268         f0 = multisensor.compute_residual(world_pts)
00269         if (self._use_cov):
00270             gamma_sqrt = multisensor.compute_marginal_gamma_sqrt(world_pts)
00271 
00272         Jt = numpy.zeros([len(x0),len(f0)])
00273         dx = numpy.zeros(len(x0))
00274         for i in range(len(x0)):
00275             dx[i] = epsilon
00276             test_vec = x0 + dx
00277             fTest = multisensor.compute_residual(SingleTransform(test_vec).transform * local_cb_points)
00278             Jt[i] = (fTest - f0)/epsilon
00279             #import code; code.interact(local=locals())
00280             dx[i] = 0.0
00281         J = Jt.transpose()
00282         if (self._use_cov):
00283             J_scaled = gamma_sqrt * J
00284         else:
00285             J_scaled = J
00286         return J_scaled
00287 
00288 def build_opt_vector(robot_params, free_dict, pose_guess_arr):
00289     """
00290     Construct vector of all the parameters that we're optimizing over. This includes
00291     both the free system parameters and the checkerboard poses
00292     Inputs:
00293     - robot_params: Dictionary with all of the system parameters
00294     - free_dict: Dictionary that specifies which system parameters are free
00295     - pose_guess_arr: Mx6 array storing the current checkerboard poses, where M is
00296                       the number of calibration samples
00297     Returns: Vector of length (F + Mx6), where F is the number of 1's in the free_dict
00298     """
00299 
00300     # Convert the robot parameter dictionary into a vector
00301     expanded_param_vec = robot_params.deflate()
00302     free_list = robot_params.calc_free(free_dict)
00303 
00304     # Pull out only the free system parameters from the system parameter vector
00305     opt_param_vec = array(expanded_param_vec[numpy.where(free_list)].T)[0]
00306 
00307     assert(pose_guess_arr.shape[1] == 6)
00308     opt_pose_vec = reshape(pose_guess_arr, [-1])
00309 
00310     return numpy.concatenate([opt_param_vec, opt_pose_vec])
00311 
00312 def compute_errors_breakdown(error_calc, multisensors, opt_pose_arr):
00313     errors_dict = {}
00314     # Compute the error for each sensor type
00315     for ms, k in zip(multisensors, range(len(multisensors))):
00316         cb = error_calc._robot_params.checkerboards[ms.checkerboard]
00317         cb_pose_vec = opt_pose_arr[k]
00318         target_pts = SingleTransform(cb_pose_vec).transform * cb.generate_points()
00319         for sensor in ms.sensors:
00320             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
00321             if sensor.sensor_id not in errors_dict.keys():
00322                 errors_dict[sensor.sensor_id] = []
00323             errors_dict[sensor.sensor_id].append(r_sensor)
00324     return errors_dict
00325 
00326 def opt_runner(robot_params, pose_guess_arr, free_dict, multisensors, use_cov):
00327     """
00328     Runs a single optimization step for the calibration optimization.
00329       robot_params - Instance of UrdfParams
00330       free_dict - Dictionary storing which parameters are free
00331       multisensor - list of list of measurements. Each multisensor corresponds to a single checkerboard pose
00332       pose_guesses - List of guesses as to where all the checkerboard are. This is used to initialze the optimization
00333     """
00334     error_calc = ErrorCalc(robot_params, free_dict, multisensors, use_cov)
00335 
00336     # Construct the initial guess
00337     opt_all = build_opt_vector(robot_params, free_dict, pose_guess_arr)
00338 
00339     x, cov_x, infodict, mesg, iter = scipy.optimize.leastsq(error_calc.calculate_error, opt_all, Dfun=error_calc.calculate_jacobian, full_output=1)
00340 
00341     J = error_calc.calculate_jacobian(x)
00342 
00343     # A hacky way to inflate x back into robot params
00344     opt_param_vec, pose_vec = error_calc.split_all(x)
00345     expanded_param_vec = error_calc.calculate_full_param_vec(opt_param_vec)
00346     opt_pose_arr = reshape(pose_vec, [-1, 6])
00347 
00348     output_dict = error_calc._robot_params.params_to_config(expanded_param_vec)
00349 
00350     errors_dict = compute_errors_breakdown(error_calc, multisensors, opt_pose_arr)
00351 
00352     print ""
00353     print "Errors Breakdown:"
00354     for sensor_id, error_list in errors_dict.items():
00355         error_cat = numpy.concatenate(error_list)
00356         rms_error = numpy.sqrt( numpy.mean(error_cat**2) )
00357         print "  %s: %.6f" % (sensor_id, rms_error)
00358 
00359     return output_dict, opt_pose_arr, J
00360 


calibration_estimation
Author(s): Vijay Pradeep, Michael Ferguson
autogenerated on Sun Oct 5 2014 22:44:09