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 import roslib; roslib.load_manifest('camera_pose_calibration')
00029
00030 import PyKDL
00031 from tf_conversions import posemath
00032 from camera_pose_calibration.msg import CalibrationEstimate
00033 from camera_pose_calibration.msg import CameraPose
00034 from tf.msg import tfMessage
00035 from geometry_msgs.msg import TransformStamped, Transform
00036 from numpy import *
00037 from copy import deepcopy
00038 import rospy
00039
00040 pose_width = 6
00041 feature_width = 2
00042
00043
00044 def enhance(cal_samples, prior_estimate, num_iterations = 20, step_size = 0.5):
00045 set_printoptions(linewidth=4000, precision=4, threshold=5000000000000000000000000000, suppress=True)
00046 next_estimate = deepcopy(prior_estimate)
00047 lam = 1.0
00048 for i in range(num_iterations):
00049 residual, J = calculate_residual_and_jacobian(cal_samples, next_estimate)
00050 Jpinv = linalg.pinv(J, 1e-5)
00051 step = Jpinv*residual
00052 next_estimate = oplus(next_estimate, step*step_size)
00053 fixup_state(next_estimate)
00054 rospy.loginfo("RMS: %.16f"% rms(residual))
00055 if rospy.is_shutdown():
00056 return next_estimate
00057 return next_estimate
00058
00059
00060 def pose_to_transform(pose, name, time):
00061 tr = Transform()
00062 tr.translation.x = pose.position.x
00063 tr.translation.y = pose.position.y
00064 tr.translation.z = pose.position.z
00065 tr.rotation.x = pose.orientation.x
00066 tr.rotation.y = pose.orientation.y
00067 tr.rotation.z = pose.orientation.z
00068 tr.rotation.w = pose.orientation.w
00069
00070 trs = TransformStamped()
00071 trs.transform = tr
00072 trs.header.stamp = time
00073 trs.header.frame_id = 'world'
00074 trs.child_frame_id = name
00075 return trs
00076
00077
00078 def to_tf(estimate):
00079 time = rospy.Time.now()
00080 msg = tfMessage()
00081 for camera in estimate.cameras:
00082 msg.transforms.append(pose_to_transform(camera.pose, camera.camera_id, time))
00083 for target, target_id in zip(estimate.targets, range(len(estimate.targets))):
00084 msg.transforms.append(pose_to_transform(target, 'target_%d'%target_id, time))
00085 return msg
00086
00087
00088 def rms(residual):
00089 return sqrt(rms_sq(residual))
00090
00091 def rms_sq(residual):
00092 return sum(array(residual) * array(residual) / len(residual))
00093
00094
00095 def pose_oplus(kdl_pose, step):
00096 t = PyKDL.Twist()
00097 for i in range(pose_width):
00098 t[i] = step[i,0]
00099 next_pose = kdl_pose * PyKDL.addDelta(PyKDL.Frame(), t, 1.0)
00100 return next_pose
00101
00102
00103 def oplus(cur_estimate, step):
00104 result = deepcopy(cur_estimate)
00105
00106
00107 for camera, res, camera_index in zip(cur_estimate.cameras, result.cameras, [r*pose_width for r in range(len(cur_estimate.cameras))]):
00108 res.pose = posemath.toMsg(pose_oplus(posemath.fromMsg(camera.pose), step[camera_index:camera_index+pose_width]))
00109
00110 for i, target in enumerate(cur_estimate.targets):
00111 target_index = (len(cur_estimate.cameras) + i) * pose_width
00112 result.targets[i] = posemath.toMsg(pose_oplus(posemath.fromMsg(target), step[target_index:target_index+pose_width]))
00113
00114 return result
00115
00116 def normalize_quat(q):
00117 d = sqrt(q.x**2 + q.y**2 + q.z**2 + q.w**2)
00118 q.x /= d
00119 q.y /= d
00120 q.z /= d
00121 q.w /= d
00122
00123
00124 def fixup_state(estimate):
00125 for camera in estimate.cameras:
00126 normalize_quat(camera.pose.orientation)
00127
00128 for target in estimate.targets:
00129 normalize_quat(target.orientation)
00130
00131 def calculate_residual_and_jacobian(cal_samples, cur_estimate):
00132 """
00133 returns the full residual vector and jacobian
00134 """
00135
00136 num_cols = len(cur_estimate.cameras) * pose_width + len(cur_estimate.targets) * pose_width
00137 num_rows = sum ([ sum([ len(cam.features.image_points) for cam in cur_sample.M_cam]) for cur_sample in cal_samples]) * feature_width
00138
00139 J = matrix(zeros([num_rows, num_cols]))
00140 residual = matrix(zeros([num_rows, 1]))
00141
00142 cam_pose_dict = dict( [ (cur_camera.camera_id, posemath.fromMsg(cur_camera.pose)) for cur_camera in cur_estimate.cameras] )
00143 cam_index_dict = dict( [ (cur_camera.camera_id, cur_index) for cur_camera, cur_index in zip ( cur_estimate.cameras, range(len(cur_estimate.cameras)) )] )
00144
00145
00146 cur_row = 0;
00147
00148 for cur_sample, target_pose_msg, target_index in zip(cal_samples, cur_estimate.targets, range(len(cur_estimate.targets))):
00149 for cam_measurement in cur_sample.M_cam:
00150
00151 try:
00152 cam_pose = cam_pose_dict[cam_measurement.camera_id]
00153 cam_index = cam_index_dict[cam_measurement.camera_id]
00154 except KeyError:
00155 print "Couldn't find current camera_id in cam_pose dictionary"
00156 print " camera_id = %s", cam_measurement.camera_id
00157 print " %s", cam_pose_dict.keys()
00158 raise
00159
00160
00161 target_pose = posemath.fromMsg(target_pose_msg)
00162
00163 end_row = cur_row + len(cam_measurement.features.image_points)*feature_width
00164
00165
00166 target_pts = matrix([ [pt.x, pt.y, pt.z, 1.0] for pt in cam_measurement.features.object_points ]).transpose()
00167
00168
00169 measurement_vec = matrix( concatenate([ [cur_pt.x, cur_pt.y] for cur_pt in cam_measurement.features.image_points]) ).T
00170 expected_measurement = sub_h(cam_pose, target_pose, target_pts, cam_measurement.cam_info)
00171 residual[cur_row:end_row, 0] = measurement_vec - expected_measurement
00172
00173
00174 camera_J = calculate_sub_jacobian(cam_pose, target_pose, target_pts, cam_measurement.cam_info, use_cam = True)
00175
00176
00177 target_J = calculate_sub_jacobian(cam_pose, target_pose, target_pts, cam_measurement.cam_info, use_cam = False)
00178 J[cur_row:end_row, cam_index*pose_width:((cam_index+1)*pose_width)] = camera_J
00179 col_start = (len(cur_estimate.cameras) + target_index)*pose_width
00180 J[cur_row:end_row, col_start:(col_start+pose_width)] = target_J
00181
00182 cur_row = end_row
00183 return residual, J
00184
00185
00186 def calculate_sub_jacobian(cam_pose, target_pose, target_pts, cam_info, use_cam):
00187 """
00188 use_cam: True -> compute J for the camera pose
00189 False -> compute J for the target pose
00190 """
00191
00192 target_pose_1 = kdlcopy(target_pose)
00193 cam_pose_1 = kdlcopy(cam_pose)
00194
00195 m0 = sub_h(cam_pose, target_pose, target_pts, cam_info)
00196 J = matrix(zeros([ target_pts.shape[1]*feature_width, pose_width]))
00197
00198 eps = 1e-8
00199 for axis in range(pose_width):
00200 step = matrix(zeros([pose_width,1]))
00201 step[axis,0] = eps;
00202
00203 if use_cam:
00204 cam_pose_1 = pose_oplus(cam_pose, step)
00205
00206 else:
00207 target_pose_1 = pose_oplus(target_pose, step)
00208
00209 m1 = sub_h(cam_pose_1, target_pose_1, target_pts, cam_info)
00210
00211
00212
00213
00214 J[:, axis] = (m1 - m0) / eps
00215
00216 return J
00217
00218 def kdlcopy(pose):
00219 f = PyKDL.Frame()
00220 for i in range(3):
00221 f.p[i] = pose.p[i]
00222 for i in range(3):
00223 for j in range(3):
00224 f.M[i,j] = pose.M[i,j]
00225 return f
00226
00227
00228 def sub_h(cam_pose, target_pose, target_pts, cam_info):
00229 '''
00230 P: 3x4 Camera Projection Matrix
00231 target_pts: 4xN matrix, storing feature points of the target, in homogeneous coords
00232 return: 2Nx1 matrix of pixel coordinates
00233 '''
00234
00235 P = reshape( matrix(cam_info.P, float), (3,4) )
00236
00237 P_full = P * to4x4(cam_pose.Inverse() * target_pose)
00238
00239
00240 pixel_pts_h = P_full * target_pts
00241
00242
00243
00244 pixel_pts_flat = reshape( concatenate( [ pixel_pts_h[0, :]/pixel_pts_h[2, :],
00245 pixel_pts_h[1, :]/pixel_pts_h[2, :]], 0 ).T, [-1,1] )
00246
00247 return pixel_pts_flat
00248
00249 def to4x4(kdl_frame):
00250
00251 T = matrix( zeros((4,4), float ))
00252 T[3,3] = 1.0
00253
00254
00255 T[0:3,3] = matrix([ kdl_frame.p[0], kdl_frame.p[1], kdl_frame.p[2] ]).T
00256
00257
00258 R = T[0:3,0:3]
00259 for i in range(3):
00260 for j in range(3):
00261 R[i,j] = kdl_frame.M[i,j]
00262
00263
00264 return T
00265