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 import rospy
00044 from visualization_msgs.msg import Marker, MarkerArray
00045 import geometry_msgs.msg
00046 
00047 class ErrorCalc:
00048     """
00049     Helpers for computing errors and jacobians
00050     """
00051     def __init__(self, robot_params, free_dict, multisensors, use_cov):
00052         self._robot_params = robot_params
00053         self._expanded_params = robot_params.deflate()
00054         self._free_list = robot_params.calc_free(free_dict)
00055         self._multisensors = multisensors
00056         self._use_cov = use_cov
00057         self._j_count = 0
00058         self.marker_pub = rospy.Publisher("pose_guesses", Marker)
00059 
00060 
00061     def calculate_full_param_vec(self, opt_param_vec):
00062         '''
00063         Take the set of optimization params, and expand it into the bigger param vec
00064         '''
00065         full_param_vec = self._expanded_params.copy()
00066         full_param_vec[numpy.where(self._free_list), 0] = opt_param_vec
00067 
00068         return full_param_vec
00069 
00070     def calculate_error(self, opt_all_vec):
00071         opt_param_vec, full_pose_arr = self.split_all(opt_all_vec)
00072 
00073         full_param_vec = self.calculate_full_param_vec(opt_param_vec)
00074 
00075         # Update the primitives with the new set of parameters
00076         self._robot_params.inflate(full_param_vec)
00077 
00078         # Update all the blocks' configs
00079         for multisensor in self._multisensors:
00080             multisensor.update_config(self._robot_params)
00081 
00082         r_list = []
00083         id = 0
00084         for multisensor, cb_pose_vec in zip(self._multisensors, list(full_pose_arr)):
00085             # Process cb pose
00086             cb_points = SingleTransform(cb_pose_vec).transform * self._robot_params.checkerboards[multisensor.checkerboard].generate_points()
00087             if (self._use_cov):
00088                 r_list.append(multisensor.compute_residual_scaled(cb_points))
00089             else:
00090                 r_list.append(multisensor.compute_residual(cb_points))
00091 
00092             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]   
00093             
00094             m = Marker()
00095             m.header.frame_id = self._robot_params.base_link
00096             m.ns = "points_3d"
00097             m.id = id
00098             m.type = Marker.SPHERE_LIST
00099             m.action = Marker.MODIFY
00100             m.points = cb_points_msgs
00101             m.color.r = 1.0
00102             m.color.g = 0.0
00103             m.color.b = 1.0
00104             m.color.a = 1.0
00105             m.scale.x = 0.01
00106             m.scale.y = 0.01
00107             m.scale.z = 0.01
00108             self.marker_pub.publish(m)
00109             
00110             cur_pt = cb_points.T[0]
00111             
00112             m_id = Marker()
00113             m_id.header.frame_id = self._robot_params.base_link
00114             m_id.ns = "points_ids"
00115             m_id.id = id + 10000
00116             m_id.type = Marker.TEXT_VIEW_FACING
00117             m_id.action = Marker.MODIFY
00118             m_id.text = str(id)
00119             m_id.color.r = 1.0
00120             m_id.color.g = 0.0
00121             m_id.color.b = 0.0
00122             m_id.color.a = 1.0
00123             m_id.scale.x = 0.06
00124             m_id.scale.y = 0.06
00125             m_id.scale.z = 0.06
00126             m_id.pose.position.x = cur_pt[0,0];
00127             m_id.pose.position.y = cur_pt[0,1];
00128             m_id.pose.position.z = cur_pt[0,2] + 0.05;
00129             m_id.pose.orientation.x = 0.0;
00130             m_id.pose.orientation.y = 0.0;
00131             m_id.pose.orientation.z = 0.0;
00132             m_id.pose.orientation.w = 1.0;
00133             self.marker_pub.publish(m_id)
00134             
00135             id += 1
00136                 
00137         r_vec = concatenate(r_list)
00138 
00139         rms_error = numpy.sqrt( numpy.mean(r_vec**2) )
00140         
00141         print "\t\t\t\t\tRMS error: %.3f    \r" % rms_error,
00142         sys.stdout.flush()
00143 
00144         return array(r_vec)
00145 
00146     def calculate_jacobian(self, opt_all_vec):
00147         """
00148         Full Jacobian:
00149           The resulting returned jacobian can be thought of as a set of several concatenated jacobians as follows:
00150 
00151                 [ J_multisensor_0 ]
00152             J = [ J_multisensor_1 ]
00153                 [        :        ]
00154                 [ J_multisensor_M ]
00155 
00156           where M is the number of multisensors, which is generally the number of calibration samples collected.
00157 
00158         Multisensor Jacobian:
00159           The Jacobian for a given multisensor is created by concatenating jacobians for each sensor
00160 
00161                             [ J_sensor_m_0 ]
00162           J_multisensor_m = [ J_sensor_m_1 ]
00163                             [      :       ]
00164                             [ J_sensor_m_S ]
00165 
00166           where S is the number of sensors in this multisensor.
00167 
00168         Sensor Jacobian:
00169           A single sensor jacobian is created by concatenating jacobians for the system parameters and checkerboards
00170 
00171           J_sensor_m_s = [ J_params_m_s | J_m_s_pose0 | J_m_s_pose1 | --- | J_m_s_CB_poseC ]
00172 
00173           The number of rows in J_sensor_m_s is equal to the number of rows in this sensor's residual.
00174           J_params_m_s shows how modifying the free system parameters affects the residual.
00175           Each J_m_s_pose_c is 6 columns wide and shows how moving a certain target affects the residual. All
00176             of the J_m_s_pose blocks are zero, except J_sensor_pose_m, since target m is the only target that
00177             was viewed by the sensors in this multisensor.
00178         """
00179         self._j_count += 1;
00180         sys.stdout.write("                     \r")
00181         sys.stdout.write("Computing Jacobian.. (cycle #%d)\r" % self._j_count)
00182         sys.stdout.flush()
00183         #import scipy.optimize.slsqp.approx_jacobian as approx_jacobian
00184         #J = approx_jacobian(opt_param_vec, self.calculate_error, 1e-6)
00185 
00186         opt_param_vec, full_pose_arr = self.split_all(opt_all_vec)
00187 
00188         # Allocate the full jacobian matrix
00189         ms_r_len = [ms.get_residual_length() for ms in self._multisensors]
00190         J = zeros([sum(ms_r_len), len(opt_all_vec)])
00191 
00192         # Calculate at which row each multisensor starts and ends
00193         ms_end_row = list(cumsum(ms_r_len))
00194         ms_start_row = [0] + ms_end_row[:-1]
00195 
00196         # Calculate at which column each multisensor starts and ends
00197         ms_end_col = list(cumsum([6]*len(self._multisensors)) + len(opt_param_vec))
00198         ms_start_col = [x-6 for x in ms_end_col]
00199 
00200 
00201         for i,ms in enumerate(self._multisensors):
00202             # Populate the parameter section for this multisensor
00203             J_ms_params = J[ ms_start_row[i]:ms_end_row[i],
00204                              0:len(opt_param_vec) ]
00205             s_r_len = [s.get_residual_length() for s in ms.sensors]
00206             s_end_row = list(cumsum(s_r_len))
00207             s_start_row = [0] + s_end_row[:-1]
00208             target_pose_T = SingleTransform(full_pose_arr[i,:]).transform
00209             # Fill in parameter section one sensor at a time
00210             for k,s in enumerate(ms.sensors):
00211                 J_s_params = J_ms_params[ s_start_row[k]:s_end_row[k], :]
00212                 J_s_params[:,:] = self.single_sensor_params_jacobian(opt_param_vec, target_pose_T, ms.checkerboard, s)
00213 
00214             # Populate the pose section for this multisensor
00215             J_ms_pose = J[ ms_start_row[i]:ms_end_row[i],
00216                            ms_start_col[i]:ms_end_col[i]]
00217             assert(J_ms_pose.shape[1] == 6)
00218             J_ms_pose[:,:] = self.multisensor_pose_jacobian(opt_param_vec, full_pose_arr[i,:], ms)
00219 
00220         sys.stdout.flush()
00221 
00222         return J
00223 
00224     def split_all(self, opt_all_vec):
00225         """
00226         Splits the input vector into two parts:
00227           1) opt_param_vec: The system parameters that we're optimizing,
00228           2) full_pose_arr: A Nx6 array where each row is a checkerboard pose
00229         """
00230         opt_param_len = sum(self._free_list)
00231         opt_param_vec = opt_all_vec[0:opt_param_len]
00232 
00233         full_pose_vec  = opt_all_vec[opt_param_len:]
00234         full_pose_arr = numpy.reshape(full_pose_vec, [-1,6])
00235         return opt_param_vec, full_pose_arr
00236 
00237     def single_sensor_params_jacobian(self, opt_param_vec, target_pose_T, target_id, sensor):
00238         """
00239         Computes the jacobian from the free system parameters to a single sensor
00240         Inputs:
00241         - opt_param_vec: Vector of the free system parameters (the parameters we're actuall optimizing)
00242         - target_pose_T: The pose of the target that this sensor is viewing
00243         - target_id: String name of the target that this sensor is viewing. This is necessary to generate the
00244                      set of feature points on the target
00245         - sensor: The actual sensor definition
00246         Output:
00247         - J_scaled: An mxn jacobian, where m is the length of sensor's residual and n is the length of opt_param_vec.
00248                     If covariance calculations are enabled, then the Jacobian is scaled by sqrt(Gamma), where Gamma
00249                     is the information matrix for this measurement.
00250         """
00251         sparsity_dict = sensor.build_sparsity_dict()
00252         required_keys = ['chains', 'tilting_lasers', 'transforms', 'rectified_cams', 'checkerboards']
00253         for cur_key in required_keys:
00254             if cur_key not in sparsity_dict.keys():
00255                 sparsity_dict[cur_key] = {}
00256         # Generate the full sparsity vector
00257         full_sparsity_list = self._robot_params.calc_free(sparsity_dict)
00258         full_sparsity_vec = numpy.array(full_sparsity_list)
00259 
00260         # Extract the sparsity for only the parameters we are optimizing over
00261 
00262         opt_sparsity_vec = full_sparsity_vec[numpy.where(self._free_list)].copy()
00263 
00264         # Update the primitives with the new set of parameters
00265         full_param_vec = self.calculate_full_param_vec(opt_param_vec)
00266         self._robot_params.inflate(full_param_vec)
00267         sensor.update_config(self._robot_params)
00268 
00269         # based on code from scipy.slsqp
00270         x0 = opt_param_vec
00271         epsilon = 1e-6
00272         target_points = target_pose_T * self._robot_params.checkerboards[target_id].generate_points()
00273         f0 = sensor.compute_residual(target_points)
00274         if (self._use_cov):
00275             gamma_sqrt = sensor.compute_marginal_gamma_sqrt(target_points)
00276         Jt = numpy.zeros([len(x0),len(f0)])
00277         dx = numpy.zeros(len(x0))
00278         for i in numpy.where(opt_sparsity_vec)[0]:
00279             dx[i] = epsilon
00280             opt_test_param_vec = x0 + dx
00281             full_test_param_vec = self.calculate_full_param_vec(opt_test_param_vec)
00282             self._robot_params.inflate(full_test_param_vec)
00283             sensor.update_config(self._robot_params)
00284             target_points = target_pose_T * self._robot_params.checkerboards[target_id].generate_points()
00285             Jt[i] = (sensor.compute_residual(target_points) - f0)/epsilon
00286             dx[i] = 0.0
00287         J = Jt.transpose()
00288         if (self._use_cov):
00289             J_scaled = gamma_sqrt * J
00290         else:
00291             J_scaled = J
00292         return J_scaled
00293 
00294     def multisensor_pose_jacobian(self, opt_param_vec, pose_param_vec, multisensor):
00295         """
00296         Generates the jacobian from a target pose to the multisensor's residual.
00297 
00298         Input:
00299         - opt_param_vec: Vector of the free system parameters (the parameters we're actuall optimizing)
00300         - pose_param_vec: Vector of length 6 encoding the target's pose 0:3=translation 3:6=rotation_axis
00301         - multisensor: The actual multisensor definition.
00302         Output:
00303         - J_scaled: An mx6 jacobian, where m is the length of multisensor's residual. There are 6 columns since the
00304                     target's pose is encoded using 6 parameters.
00305                     If covariance calculations are enabled, then the Jacobian is scaled by sqrt(Gamma), where Gamma
00306                     is the information matrix for this measurement.
00307         """
00308         # Update the primitives with the new set of parameters
00309         full_param_vec = self.calculate_full_param_vec(opt_param_vec)
00310         self._robot_params.inflate(full_param_vec)
00311         multisensor.update_config(self._robot_params)
00312         cb_model = self._robot_params.checkerboards[multisensor.checkerboard]
00313         local_cb_points = cb_model.generate_points()
00314 
00315         # based on code from scipy.slsqp
00316         x0 = pose_param_vec
00317         epsilon = 1e-6
00318         world_pts = SingleTransform(x0).transform * local_cb_points
00319         f0 = multisensor.compute_residual(world_pts)
00320         if (self._use_cov):
00321             gamma_sqrt = multisensor.compute_marginal_gamma_sqrt(world_pts)
00322 
00323         Jt = numpy.zeros([len(x0),len(f0)])
00324         dx = numpy.zeros(len(x0))
00325         for i in range(len(x0)):
00326             dx[i] = epsilon
00327             test_vec = x0 + dx
00328             fTest = multisensor.compute_residual(SingleTransform(test_vec).transform * local_cb_points)
00329             Jt[i] = (fTest - f0)/epsilon
00330             #import code; code.interact(local=locals())
00331             dx[i] = 0.0
00332         J = Jt.transpose()
00333         if (self._use_cov):
00334             J_scaled = gamma_sqrt * J
00335         else:
00336             J_scaled = J
00337         return J_scaled
00338 
00339 def build_opt_vector(robot_params, free_dict, pose_guess_arr):
00340     """
00341     Construct vector of all the parameters that we're optimizing over. This includes
00342     both the free system parameters and the checkerboard poses
00343     Inputs:
00344     - robot_params: Dictionary with all of the system parameters
00345     - free_dict: Dictionary that specifies which system parameters are free
00346     - pose_guess_arr: Mx6 array storing the current checkerboard poses, where M is
00347                       the number of calibration samples
00348     Returns: Vector of length (F + Mx6), where F is the number of 1's in the free_dict
00349     """
00350 
00351     # Convert the robot parameter dictionary into a vector
00352     expanded_param_vec = robot_params.deflate()
00353     free_list = robot_params.calc_free(free_dict)
00354 
00355     # Pull out only the free system parameters from the system parameter vector
00356     opt_param_vec = array(expanded_param_vec[numpy.where(free_list)].T)[0]
00357 
00358     assert(pose_guess_arr.shape[1] == 6)
00359     opt_pose_vec = reshape(pose_guess_arr, [-1])
00360 
00361     return numpy.concatenate([opt_param_vec, opt_pose_vec])
00362 
00363 def compute_errors_breakdown(error_calc, multisensors, opt_pose_arr):
00364     errors_dict = {}
00365     # Compute the error for each sensor type
00366     for ms, k in zip(multisensors, range(len(multisensors))):
00367         cb = error_calc._robot_params.checkerboards[ms.checkerboard]
00368         cb_pose_vec = opt_pose_arr[k]
00369         target_pts = SingleTransform(cb_pose_vec).transform * cb.generate_points()
00370         for sensor in ms.sensors:
00371             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
00372             if sensor.sensor_id not in errors_dict.keys():
00373                 errors_dict[sensor.sensor_id] = []
00374             errors_dict[sensor.sensor_id].append(r_sensor)
00375     return errors_dict
00376 
00377 def opt_runner(robot_params, pose_guess_arr, free_dict, multisensors, use_cov):
00378     """
00379     Runs a single optimization step for the calibration optimization.
00380       robot_params - Instance of UrdfParams
00381       free_dict - Dictionary storing which parameters are free
00382       multisensor - list of list of measurements. Each multisensor corresponds to a single checkerboard pose
00383       pose_guesses - List of guesses as to where all the checkerboard are. This is used to initialze the optimization
00384     """
00385     error_calc = ErrorCalc(robot_params, free_dict, multisensors, use_cov)
00386 
00387     # Construct the initial guess
00388     opt_all = build_opt_vector(robot_params, free_dict, pose_guess_arr)
00389 
00390     x, cov_x, infodict, mesg, iter = scipy.optimize.leastsq(error_calc.calculate_error, opt_all, Dfun=error_calc.calculate_jacobian, full_output=1)
00391 
00392     J = error_calc.calculate_jacobian(x)
00393 
00394     # A hacky way to inflate x back into robot params
00395     opt_param_vec, pose_vec = error_calc.split_all(x)
00396     expanded_param_vec = error_calc.calculate_full_param_vec(opt_param_vec)
00397     opt_pose_arr = reshape(pose_vec, [-1, 6])
00398 
00399     output_dict = error_calc._robot_params.params_to_config(expanded_param_vec)
00400 
00401     errors_dict = compute_errors_breakdown(error_calc, multisensors, opt_pose_arr)
00402 
00403     print ""
00404     print "Errors Breakdown:"
00405     for sensor_id, error_list in errors_dict.items():
00406         error_cat = numpy.concatenate(error_list)
00407         rms_error = numpy.sqrt( numpy.mean(error_cat**2) )
00408         print "  %s: %.6f" % (sensor_id, rms_error)
00409 
00410     return output_dict, opt_pose_arr, J
00411 


calibration_estimation
Author(s): Vijay Pradeep, Michael Ferguson
autogenerated on Tue Sep 27 2016 04:06:33