00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
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
00055
00056 def calculate_full_param_vec(self, opt_param_vec):
00057 '''
00058 Take the set of optimization params, and expand it into the bigger param vec
00059 '''
00060 full_param_vec = self._expanded_params.copy()
00061 full_param_vec[numpy.where(self._free_list), 0] = opt_param_vec
00062
00063 return full_param_vec
00064
00065 def calculate_error(self, opt_all_vec):
00066 print "x ",
00067 sys.stdout.flush()
00068
00069 opt_param_vec, full_pose_arr = self.split_all(opt_all_vec)
00070
00071 full_param_vec = self.calculate_full_param_vec(opt_param_vec)
00072
00073
00074 self._robot_params.inflate(full_param_vec)
00075
00076
00077 for multisensor in self._multisensors:
00078 multisensor.update_config(self._robot_params)
00079
00080 r_list = []
00081 for multisensor, cb_pose_vec in zip(self._multisensors, list(full_pose_arr)):
00082
00083 cb_points = SingleTransform(cb_pose_vec).transform * self._robot_params.checkerboards[multisensor.checkerboard].generate_points()
00084 if (self._use_cov):
00085 r_list.append(multisensor.compute_residual_scaled(cb_points))
00086 else:
00087 r_list.append(multisensor.compute_residual(cb_points))
00088 r_vec = concatenate(r_list)
00089
00090 rms_error = numpy.sqrt( numpy.mean(r_vec**2) )
00091 print "%.3f " % rms_error,
00092 sys.stdout.flush()
00093
00094 return array(r_vec)
00095
00096 def calculate_jacobian(self, opt_all_vec):
00097 """
00098 Full Jacobian:
00099 The resulting returned jacobian can be thought of as a set of several concatenated jacobians as follows:
00100
00101 [ J_multisensor_0 ]
00102 J = [ J_multisensor_1 ]
00103 [ : ]
00104 [ J_multisensor_M ]
00105
00106 where M is the number of multisensors, which is generally the number of calibration samples collected.
00107
00108 Multisensor Jacobian:
00109 The Jacobian for a given multisensor is created by concatenating jacobians for each sensor
00110
00111 [ J_sensor_m_0 ]
00112 J_multisensor_m = [ J_sensor_m_1 ]
00113 [ : ]
00114 [ J_sensor_m_S ]
00115
00116 where S is the number of sensors in this multisensor.
00117
00118 Sensor Jacobian:
00119 A single sensor jacobian is created by concatenating jacobians for the system parameters and checkerboards
00120
00121 J_sensor_m_s = [ J_params_m_s | J_m_s_pose0 | J_m_s_pose1 | --- | J_m_s_CB_poseC ]
00122
00123 The number of rows in J_sensor_m_s is equal to the number of rows in this sensor's residual.
00124 J_params_m_s shows how modifying the free system parameters affects the residual.
00125 Each J_m_s_pose_c is 6 columns wide and shows how moving a certain target affects the residual. All
00126 of the J_m_s_pose blocks are zero, except J_sensor_pose_m, since target m is the only target that
00127 was viewed by the sensors in this multisensor.
00128 """
00129 sys.stdout.write("J-")
00130 sys.stdout.flush()
00131
00132
00133
00134 opt_param_vec, full_pose_arr = self.split_all(opt_all_vec)
00135
00136
00137 ms_r_len = [ms.get_residual_length() for ms in self._multisensors]
00138 J = zeros([sum(ms_r_len), len(opt_all_vec)])
00139
00140
00141 ms_end_row = list(cumsum(ms_r_len))
00142 ms_start_row = [0] + ms_end_row[:-1]
00143
00144
00145 ms_end_col = list(cumsum([6]*len(self._multisensors)) + len(opt_param_vec))
00146 ms_start_col = [x-6 for x in ms_end_col]
00147
00148
00149 for i,ms in enumerate(self._multisensors):
00150
00151 J_ms_params = J[ ms_start_row[i]:ms_end_row[i],
00152 0:len(opt_param_vec) ]
00153 s_r_len = [s.get_residual_length() for s in ms.sensors]
00154 s_end_row = list(cumsum(s_r_len))
00155 s_start_row = [0] + s_end_row[:-1]
00156 target_pose_T = SingleTransform(full_pose_arr[i,:]).transform
00157
00158 for k,s in enumerate(ms.sensors):
00159 J_s_params = J_ms_params[ s_start_row[k]:s_end_row[k], :]
00160 J_s_params[:,:] = self.single_sensor_params_jacobian(opt_param_vec, target_pose_T, ms.checkerboard, s)
00161
00162
00163 J_ms_pose = J[ ms_start_row[i]:ms_end_row[i],
00164 ms_start_col[i]:ms_end_col[i]]
00165 assert(J_ms_pose.shape[1] == 6)
00166 J_ms_pose[:,:] = self.multisensor_pose_jacobian(opt_param_vec, full_pose_arr[i,:], ms)
00167
00168 print "-J",
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
00206 full_sparsity_list = self._robot_params.calc_free(sparsity_dict)
00207 full_sparsity_vec = numpy.array(full_sparsity_list)
00208
00209
00210
00211 opt_sparsity_vec = full_sparsity_vec[numpy.where(self._free_list)].copy()
00212
00213
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
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
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
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
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
00301 expanded_param_vec = robot_params.deflate()
00302 free_list = robot_params.calc_free(free_dict)
00303
00304
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
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)
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
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
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