estimate.py
Go to the documentation of this file.
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 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


camera_pose_calibration
Author(s): Vijay Pradeep, Wim Meeussen
autogenerated on Thu Aug 15 2013 10:18:08