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
00036
00037
00038
00039
00040
00041
00042
00043
00044 import numpy
00045 from numpy import matrix, reshape, array, zeros, diag, real
00046
00047 import roslib; roslib.load_manifest('pr2_calibration_estimation')
00048 import rospy
00049 from pr2_calibration_estimation.full_chain import FullChainRobotParams
00050 from sensor_msgs.msg import JointState
00051
00052 class CameraChainBundler:
00053 """
00054 Tool used to generate a list of CameraChain sensors from a single calibration sample message
00055 """
00056 def __init__(self, valid_configs):
00057 """
00058 Inputs:
00059 - valid_configs: A list of dictionaries, where each list elem stores the configuration of a potential
00060 camera chain that we might encounter.
00061 Example (in yaml format):
00062 - camera_id: forearm_right_rect
00063 sensor_id: forearm_right_rect
00064 chain:
00065 before_chain: [r_shoulder_pan_joint]
00066 chain_id: right_arm_chain
00067 after_chain: [r_forearm_roll_adj, r_forearm_cam_frame_joint,
00068 r_forearm_cam_optical_frame_joint]
00069 dh_link_num: 4
00070 """
00071 self._valid_configs = valid_configs
00072
00073
00074 def build_blocks(self, M_robot):
00075 """
00076 Input:
00077 - M_robot: A calibration sample of type calibration_msgs/RobotMeasurement
00078 Returns:
00079 - sensors: A list of CameraChainSensor objects that corresponds to all camera chain sensors found
00080 in the calibration message that was passed in.
00081 """
00082 sensors = []
00083 for cur_config in self._valid_configs:
00084 if cur_config["camera_id"] in [ x.camera_id for x in M_robot.M_cam ]:
00085 if cur_config["chain"]["chain_id"] in [ x.chain_id for x in M_robot.M_chain ] :
00086
00087 M_cam = M_robot.M_cam [ [ x.camera_id for x in M_robot.M_cam ].index(cur_config["camera_id"])]
00088 M_chain = M_robot.M_chain[ [ x.chain_id for x in M_robot.M_chain ].index(cur_config["chain"]["chain_id"]) ]
00089
00090 elif cur_config["chain"]["chain_id"] == None:
00091
00092 M_cam = M_robot.M_cam [ [ x.camera_id for x in M_robot.M_cam ].index(cur_config["camera_id"])]
00093 M_chain = None
00094 else:
00095 print "else cur_config[chain][chain_id]: ", cur_config["chain"]["chain_id"]
00096 break
00097 cur_sensor = CameraChainSensor(cur_config, M_cam, M_chain)
00098 sensors.append(cur_sensor)
00099 else:
00100 rospy.logdebug(" Didn't find block")
00101 return sensors
00102
00103 class CameraChainSensor:
00104 def __init__(self, config_dict, M_cam, M_chain):
00105 """
00106 Generates a single sensor block for a single configuration
00107 Inputs:
00108 - config_dict: The configuration for this specific sensor. This looks exactly like
00109 a single element from the valid_configs list passed into CameraChainBundler.__init__
00110 - M_cam: The camera measurement of type calibration_msgs/CameraMeasurement
00111 - M_chain: The chain measurement of type calibration_msgs/ChainMeasurement
00112 """
00113
00114 self.sensor_type = "camera"
00115 self.sensor_id = config_dict["camera_id"]
00116
00117 self._config_dict = config_dict
00118 self._M_cam = M_cam
00119 self._M_chain = M_chain
00120
00121 self._chain = FullChainRobotParams(config_dict["chain"])
00122
00123 self.terms_per_sample = 2
00124
00125 def update_config(self, robot_params):
00126 """
00127 On each optimization cycle the set of system parameters that we're optimizing over will change. Thus,
00128 after each change in parameters we must call update_config to ensure that our calculated residual reflects
00129 the newest set of system parameters.
00130 """
00131 self._camera = robot_params.rectified_cams[ self._config_dict["camera_id"] ]
00132
00133 if self._chain is not None:
00134 self._chain.update_config(robot_params)
00135
00136 def compute_residual(self, target_pts):
00137 """
00138 Computes the measurement residual for the current set of system parameters and target points
00139 Input:
00140 - target_pts: 4XN matrix, storing features point locations in world cartesian homogenous coordinates.
00141 Output:
00142 - r: 2N long vector, storing pixel residuals for the target points in the form [u1, v1, u2, v2, ..., uN, vN]
00143 """
00144 z_mat = self.get_measurement()
00145 h_mat = self.compute_expected(target_pts)
00146 assert(z_mat.shape[1] == 2)
00147 assert(h_mat.shape[1] == 2)
00148 assert(z_mat.shape[0] == z_mat.shape[0])
00149 r = array(reshape(h_mat - z_mat, [-1,1]))[:,0]
00150 return r
00151
00152
00153 def compute_residual_scaled(self, target_pts):
00154 """
00155 Computes the residual, and then scales it by sqrt(Gamma), where Gamma
00156 is the information matrix for this measurement (Cov^-1).
00157 """
00158 r = self.compute_residual(target_pts)
00159 gamma_sqrt = self.compute_marginal_gamma_sqrt(target_pts)
00160 r_scaled = gamma_sqrt * matrix(r).T
00161 return array(r_scaled.T)[0]
00162
00163 def compute_marginal_gamma_sqrt(self, target_pts):
00164 """
00165 Calculates the square root of the information matrix for the measurement of the
00166 current set of system parameters at the passed in set of target points.
00167 """
00168 import scipy.linalg
00169 cov = self.compute_cov(target_pts)
00170 gamma = matrix(zeros(cov.shape))
00171 num_pts = self.get_residual_length()/2
00172
00173 for k in range(num_pts):
00174
00175 first = 2*k
00176 last = 2*k+2
00177 sub_cov = matrix(cov[first:last, first:last])
00178 sub_gamma_sqrt_full = matrix(scipy.linalg.sqrtm(sub_cov.I))
00179 sub_gamma_sqrt = real(sub_gamma_sqrt_full)
00180 assert(scipy.linalg.norm(sub_gamma_sqrt_full - sub_gamma_sqrt) < 1e-6)
00181 gamma[first:last, first:last] = sub_gamma_sqrt
00182 return gamma
00183
00184 def get_residual_length(self):
00185 N = len(self._M_cam.image_points)
00186 return N*2
00187
00188
00189 def get_measurement(self):
00190 """
00191 Get the target's pixel coordinates as measured by the actual sensor
00192 """
00193 camera_pix = numpy.matrix([[pt.x, pt.y] for pt in self._M_cam.image_points])
00194 return camera_pix
00195
00196 def compute_expected(self, target_pts):
00197 """
00198 Compute the expected pixel coordinates for a set of target points.
00199 target_pts: 4xN matrix, storing feature points of the target, in homogeneous coords
00200 Returns: target points projected into pixel coordinates, in a Nx2 matrix
00201 """
00202 if self._M_chain is not None:
00203 return self._compute_expected(self._M_chain.chain_state, target_pts)
00204 else:
00205 return self._compute_expected(None, target_pts)
00206 def _compute_expected(self, chain_state, target_pts):
00207 """
00208 Compute what measurement we would expect to see, based on the current system parameters
00209 and the specified target point locations.
00210
00211 Inputs:
00212 - chain_state: The joint angles of this sensor's chain of type sensor_msgs/JointState.
00213 - target_pts: 4XN matrix, storing features point locations in world cartesian homogenous coordinates.
00214
00215 Returns: target points projected into pixel coordinates, in a Nx2 matrix
00216 """
00217
00218 camera_pose_root = self._chain.calc_block.fk(chain_state)
00219 cam_frame_pts = camera_pose_root.I * target_pts
00220
00221 pixel_pts = self._camera.project(self._M_cam.cam_info.P, cam_frame_pts)
00222
00223 return pixel_pts.T
00224
00225 def compute_expected_J(self, target_pts):
00226 """
00227 The output Jacobian J shows how moving target_pts in cartesian space affects
00228 the expected measurement in (u,v) camera coordinates.
00229 For n points in target_pts, J is a 2nx3n matrix
00230 Note: This doesn't seem to be used anywhere, except maybe in some drawing code
00231 """
00232 epsilon = 1e-8
00233 N = len(self._M_cam.image_points)
00234 Jt = zeros([N*3, N*2])
00235 for k in range(N):
00236
00237 sub_Jt = zeros([3,2])
00238 x = target_pts[:,k].copy()
00239 f0 = self.compute_expected(x)
00240 for i in [0,1,2]:
00241 x[i,0] += epsilon
00242 fTest = self.compute_expected(x)
00243 sub_Jt[i,:] = array((fTest - f0) / epsilon)
00244 x[i,0] -= epsilon
00245 Jt[k*3:(k+1)*3, k*2:(k+1)*2] = sub_Jt
00246 return Jt.T
00247
00248
00249 def compute_cov(self, target_pts):
00250 '''
00251 Computes the measurement covariance in pixel coordinates for the given
00252 set of target points (target_pts)
00253 Input:
00254 - target_pts: 4xN matrix, storing N feature points of the target, in homogeneous coords
00255 '''
00256 epsilon = 1e-8
00257
00258 if self._M_chain is not None:
00259 num_joints = len(self._M_chain.chain_state.position)
00260 Jt = zeros([num_joints, self.get_residual_length()])
00261
00262 x = JointState()
00263 x.position = self._M_chain.chain_state.position[:]
00264
00265
00266 f0 = reshape(array(self._compute_expected(x, target_pts)), [-1])
00267 for i in range(num_joints):
00268 x.position = [cur_pos for cur_pos in self._M_chain.chain_state.position]
00269 x.position[i] += epsilon
00270 fTest = reshape(array(self._compute_expected(x, target_pts)), [-1])
00271 Jt[i] = (fTest - f0)/epsilon
00272 cov_angles = [x*x for x in self._chain.calc_block._chain._cov_dict['joint_angles']]
00273
00274
00275 chain_cov = matrix(Jt).T * matrix(diag(cov_angles)) * matrix(Jt)
00276
00277 cam_cov = matrix(zeros([self.get_residual_length(), self.get_residual_length()]))
00278
00279
00280 var_u = self._camera._cov_dict['u'] * self._camera._cov_dict['u']
00281 var_v = self._camera._cov_dict['v'] * self._camera._cov_dict['v']
00282 for k in range(cam_cov.shape[0]/2):
00283 cam_cov[2*k , 2*k] = var_u
00284 cam_cov[2*k+1, 2*k+1] = var_v
00285
00286
00287 if self._M_chain is not None:
00288 cov = chain_cov + cam_cov
00289 else:
00290 cov = cam_cov
00291 return cov
00292
00293 def build_sparsity_dict(self):
00294 """
00295 Build a dictionary that defines which parameters will in fact affect this measurement.
00296
00297 Returned dictionary should be of the following form:
00298 transforms:
00299 my_transformA: [1, 1, 1, 1, 1, 1]
00300 my_transformB: [1, 1, 1, 1, 1, 1]
00301 dh_chains:
00302 my_chain:
00303 dh:
00304 - [1, 1, 1, 1]
00305 - [1, 1, 1, 1]
00306 |
00307 - [1, 1, 1, 1]
00308 gearing: [1, 1, ---, 1]
00309 rectified_cams:
00310 my_cam:
00311 baseline_shift: 1
00312 f_shift: 1
00313 cx_shift: 1
00314 cy_shift: 1
00315 """
00316 sparsity = dict()
00317 sparsity['transforms'] = {}
00318 for cur_transform_name in ( self._config_dict['chain']['before_chain'] + self._config_dict['chain']['after_chain'] ):
00319 sparsity['transforms'][cur_transform_name] = [1, 1, 1, 1, 1, 1]
00320
00321 sparsity['dh_chains'] = {}
00322 if self._M_chain is not None:
00323 chain_id = self._config_dict['chain']['chain_id']
00324 num_links = self._chain.calc_block._chain._M
00325 assert(num_links == len(self._M_chain.chain_state.position))
00326 sparsity['dh_chains'][chain_id] = {}
00327 sparsity['dh_chains'][chain_id]['dh'] = [ [1,1,1,1] ] * num_links
00328 sparsity['dh_chains'][chain_id]['gearing'] = [1] * num_links
00329
00330 sparsity['rectified_cams'] = {}
00331 sparsity['rectified_cams'][self.sensor_id] = dict( [(x,1) for x in self._camera.get_param_names()] )
00332
00333 return sparsity
00334
00335
