chain_sensor.py
Go to the documentation of this file.
00001 # Software License Agreement (BSD License)
00002 #
00003 # Copyright (c) 2008, Willow Garage, Inc.
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions
00008 # are met:
00009 #
00010 #  * Redistributions of source code must retain the above copyright
00011 #    notice, this list of conditions and the following disclaimer.
00012 #  * Redistributions in binary form must reproduce the above
00013 #    copyright notice, this list of conditions and the following
00014 #    disclaimer in the documentation and/or other materials provided
00015 #    with the distribution.
00016 #  * Neither the name of Willow Garage, Inc. nor the names of its
00017 #    contributors may be used to endorse or promote products derived
00018 #    from this software without specific prior written permission.
00019 #
00020 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 # POSSIBILITY OF SUCH DAMAGE.
00032 
00033 # author: Vijay Pradeep
00034 
00035 # Error block for a monocular camera plus tilting laser.  The
00036 # camera is attached to a chain, but also has a 6 dof transform
00037 # in between the tilting laser is attached to the
00038 #
00039 #       before_chain_Ts -- camera_chain -- after_chain_Ts -- camera
00040 #      /
00041 #   root
00042 #      \
00043 #       before_chain_Ts -- target_chain -- after_chain_Ts -- checkerboard
00044 
00045 import numpy
00046 from numpy import reshape, array, zeros, diag, matrix, real
00047 import roslib; roslib.load_manifest('calibration_estimation')
00048 import rospy
00049 from calibration_estimation.full_chain import FullChainRobotParams
00050 from sensor_msgs.msg import JointState
00051 
00052 class ChainBundler:
00053     def __init__(self, valid_configs):
00054         self._valid_configs = valid_configs
00055 
00056     # Construct a CameraChainRobotParamsBlock for every 'valid config' that finds everything it needs in the current robot measurement
00057     def build_blocks(self, M_robot):
00058         sensors = []
00059         for cur_config in self._valid_configs:
00060             if cur_config["sensor_id"] == M_robot.chain_id and \
00061                 cur_config["sensor_id"] in [ x.chain_id  for x in M_robot.M_chain ]:
00062                 M_chain = [x for x in M_robot.M_chain if cur_config["sensor_id"] == x.chain_id][0]
00063                 cur_sensor = ChainSensor(cur_config, M_chain, M_robot.target_id)
00064                 sensors.append(cur_sensor)
00065             else:
00066                 rospy.logdebug("  Didn't find block")
00067         return sensors
00068 
00069 class ChainSensor:
00070     def __init__(self, config_dict, M_chain, target_id):
00071 
00072         self.sensor_type = "chain"
00073         self.sensor_id   = config_dict["sensor_id"]
00074 
00075         self._config_dict = config_dict
00076         self._M_chain = M_chain
00077         self._target_id = target_id
00078 
00079         self._full_chain = FullChainRobotParams(self.sensor_id, self.sensor_id+"_cb_link")
00080 
00081         self.terms_per_sample = 3
00082 
00083     def update_config(self, robot_params):
00084         self._full_chain.update_config(robot_params)
00085         self._checkerboard = robot_params.checkerboards[ self._target_id ]
00086 
00087     def compute_residual(self, target_pts):
00088         h_mat = self.compute_expected(target_pts)
00089         z_mat = self.get_measurement()
00090         assert(h_mat.shape == z_mat.shape)
00091         assert(h_mat.shape[0] == 4)
00092         r_mat = h_mat[0:3,:] - z_mat[0:3,:]
00093         r = array(reshape(r_mat.T, [-1,1]))[:,0]
00094         return r
00095 
00096     def compute_residual_scaled(self, target_pts):
00097         r = self.compute_residual(target_pts)
00098         gamma_sqrt = self.compute_marginal_gamma_sqrt(target_pts)
00099         r_scaled = gamma_sqrt * matrix(r).T
00100         return array(r_scaled.T)[0]
00101 
00102     def compute_marginal_gamma_sqrt(self, target_pts):
00103         import scipy.linalg
00104         # ----- Populate Here -----
00105         cov = self.compute_cov(target_pts)
00106         gamma = matrix(zeros(cov.shape))
00107         num_pts = self.get_residual_length()/3
00108 
00109         for k in range(num_pts):
00110             #print "k=%u" % k
00111             first = 3*k
00112             last = 3*k+3
00113             sub_cov = matrix(cov[first:last, first:last])
00114             sub_gamma_sqrt_full = matrix(scipy.linalg.sqrtm(sub_cov.I))
00115             sub_gamma_sqrt = real(sub_gamma_sqrt_full)
00116             assert(scipy.linalg.norm(sub_gamma_sqrt_full - sub_gamma_sqrt) < 1e-6)
00117             gamma[first:last, first:last] = sub_gamma_sqrt
00118         return gamma
00119 
00120     def compute_cov(self, target_pts):
00121         epsilon = 1e-8
00122 
00123         num_joints = len(self._M_chain.chain_state.position)
00124         Jt = zeros([num_joints, self.get_residual_length()])
00125 
00126         x = JointState()
00127         x.position = self._M_chain.chain_state.position[:]
00128 
00129         f0 = reshape(array(self._calc_fk_target_pts(x)[0:3,:].T), [-1])
00130         for i in range(num_joints):
00131             x.position = list(self._M_chain.chain_state.position[:])
00132             x.position[i] += epsilon
00133             fTest = reshape(array(self._calc_fk_target_pts(x)[0:3,:].T), [-1])
00134             Jt[i] = (fTest - f0)/epsilon
00135         cov_angles = [x*x for x in self._full_chain.calc_block._chain._cov_dict['joint_angles']]
00136         cov = matrix(Jt).T * matrix(diag(cov_angles)) * matrix(Jt)
00137         
00138         if ( self._full_chain.calc_block._chain._cov_dict.has_key('translation') ):
00139             translation_var = self._full_chain.calc_block._chain._cov_dict['translation'];
00140             translation_cov = numpy.diag(translation_var*(self.get_residual_length()/3))
00141             cov = cov + translation_cov
00142 
00143         return cov
00144 
00145     def get_residual_length(self):
00146         pts = self._checkerboard.generate_points()
00147         N = pts.shape[1]
00148         return N*3
00149 
00150     def get_measurement(self):
00151         '''
00152         Returns a 4xN matrix with the locations of the checkerboard points in homogenous coords,
00153         as per the forward kinematics of the chain
00154         '''
00155         return self._calc_fk_target_pts(self._M_chain.chain_state)
00156 
00157     def _calc_fk_target_pts(self, chain_state):
00158         # Get the target's model points in the frame of the tip of the target chain
00159         target_pts_tip = self._checkerboard.generate_points()
00160 
00161         # Target pose in root frame
00162         target_pose_root = self._full_chain.calc_block.fk(chain_state)
00163 
00164         # Transform points into the root frame
00165         target_pts_root = target_pose_root * target_pts_tip
00166 
00167         return target_pts_root
00168 
00169     def compute_expected(self, target_pts):
00170         return target_pts
00171 
00172     # Build a dictionary that defines which parameters will in fact affect this measurement
00173     def build_sparsity_dict(self):
00174         sparsity = self._full_chain.build_sparsity_dict()
00175         sparsity['checkerboards'] = {}
00176         sparsity['checkerboards'][self._target_id] = { 'spacing_x': 1, 'spacing_y': 1 }
00177         return sparsity
00178 


calibration_estimation
Author(s): Vijay Pradeep, Michael Ferguson
autogenerated on Wed Aug 26 2015 10:56:21