$search
00001 # Copyright (c) 2009, Willow Garage, Inc. 00002 # All rights reserved. 00003 # 00004 # Redistribution and use in source and binary forms, with or without 00005 # modification, are permitted provided that the following conditions are met: 00006 # 00007 # * Redistributions of source code must retain the above copyright 00008 # notice, this list of conditions and the following disclaimer. 00009 # * Redistributions in binary form must reproduce the above copyright 00010 # notice, this list of conditions and the following disclaimer in the 00011 # documentation and/or other materials provided with the distribution. 00012 # * Neither the name of the Willow Garage, Inc. nor the names of its 00013 # contributors may be used to endorse or promote products derived from 00014 # this software without specific prior written permission. 00015 # 00016 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00017 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00018 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00019 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00020 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00021 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00022 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00023 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00024 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00025 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00026 # POSSIBILITY OF SUCH DAMAGE. 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 # lambda 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 # loop over camera's 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 # loop over targets 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 # Normalizes quaternions 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 # Compute the total number of rows. This is the number of poses * 6 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 # Start filling in the jacobian 00146 cur_row = 0; 00147 # Loop over each observation 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 # Find the index of this camera 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 # ROS Poses -> KDL Poses 00161 target_pose = posemath.fromMsg(target_pose_msg) 00162 00163 end_row = cur_row + len(cam_measurement.features.image_points)*feature_width 00164 00165 # ROS Target Points -> (4xN) Homogenous coords 00166 target_pts = matrix([ [pt.x, pt.y, pt.z, 1.0] for pt in cam_measurement.features.object_points ]).transpose() 00167 00168 # Save the residual for this cam measurement 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 # Compute jacobian for this cam measurement 00174 camera_J = calculate_sub_jacobian(cam_pose, target_pose, target_pts, cam_measurement.cam_info, use_cam = True) 00175 #print "Camera Jacobian [%s]]" % cam_measurement.camera_id 00176 #print camera_J 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 # Decide if we're operating on camera poses or target poses 00203 if use_cam: 00204 cam_pose_1 = pose_oplus(cam_pose, step) 00205 #print "CAM POSE 1\n", cam_pose_1 00206 else: 00207 target_pose_1 = pose_oplus(target_pose, step) 00208 #print "TARGET POSE 1\n", target_pose_1 00209 m1 = sub_h(cam_pose_1, target_pose_1, target_pts, cam_info) 00210 #print "SUB_ m1 \n", m1*100000 00211 #print "SUB_ m0 \n", m0*100000 00212 #print "Diff \n", (m1 - m0)*100000 00213 #print "SUB_JAC \n", (m1- m0)/eps 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 # CameraInfo -> Numpy Camera Projection Matrix 00235 P = reshape( matrix(cam_info.P, float), (3,4) ) 00236 00237 P_full = P * to4x4(cam_pose.Inverse() * target_pose) 00238 00239 # Apply projection matrix 00240 pixel_pts_h = P_full * target_pts 00241 00242 # Strip out last row (3rd) and rescale 00243 #pixel_pts = pixel_pts_h[0:2,:] / pixel_pts_h[2,:] 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 # Init output matrix 00251 T = matrix( zeros((4,4), float )) 00252 T[3,3] = 1.0 00253 00254 # Copy position into matrix 00255 T[0:3,3] = matrix([ kdl_frame.p[0], kdl_frame.p[1], kdl_frame.p[2] ]).T 00256 00257 # Generate pointer to rotation submatrix 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 #print "to4x4 kdl: \n", kdl_frame 00263 #print "to4x4 mat: \n",T 00264 return T 00265