chain_sensor.py
Go to the documentation of this file.
1 # Software License Agreement (BSD License)
2 #
3 # Copyright (c) 2008, Willow Garage, Inc.
4 # All rights reserved.
5 #
6 # Redistribution and use in source and binary forms, with or without
7 # modification, are permitted provided that the following conditions
8 # are met:
9 #
10 # * Redistributions of source code must retain the above copyright
11 # notice, this list of conditions and the following disclaimer.
12 # * Redistributions in binary form must reproduce the above
13 # copyright notice, this list of conditions and the following
14 # disclaimer in the documentation and/or other materials provided
15 # with the distribution.
16 # * Neither the name of Willow Garage, Inc. nor the names of its
17 # contributors may be used to endorse or promote products derived
18 # from this software without specific prior written permission.
19 #
20 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 # POSSIBILITY OF SUCH DAMAGE.
32 
33 # author: Vijay Pradeep
34 
35 # Error block for a monocular camera plus tilting laser. The
36 # camera is attached to a chain, but also has a 6 dof transform
37 # in between the tilting laser is attached to the
38 #
39 # before_chain_Ts -- camera_chain -- after_chain_Ts -- camera
40 # /
41 # root
42 # \
43 # before_chain_Ts -- target_chain -- after_chain_Ts -- checkerboard
44 
45 import numpy
46 from numpy import reshape, array, zeros, diag, matrix, real
47 import roslib; roslib.load_manifest('calibration_estimation')
48 import rospy
49 from calibration_estimation.full_chain import FullChainRobotParams
50 from sensor_msgs.msg import JointState
51 
53  def __init__(self, valid_configs):
54  self._valid_configs = valid_configs
55 
56  # Construct a CameraChainRobotParamsBlock for every 'valid config' that finds everything it needs in the current robot measurement
57  def build_blocks(self, M_robot):
58  sensors = []
59  for cur_config in self._valid_configs:
60  if cur_config["sensor_id"] == M_robot.chain_id and \
61  cur_config["sensor_id"] in [ x.chain_id for x in M_robot.M_chain ]:
62  M_chain = [x for x in M_robot.M_chain if cur_config["sensor_id"] == x.chain_id][0]
63  cur_sensor = ChainSensor(cur_config, M_chain, M_robot.target_id)
64  sensors.append(cur_sensor)
65  else:
66  rospy.logdebug(" Didn't find block")
67  return sensors
68 
70  def __init__(self, config_dict, M_chain, target_id):
71 
72  self.sensor_type = "chain"
73  self.sensor_id = config_dict["sensor_id"]
74 
75  self._config_dict = config_dict
76  self._M_chain = M_chain
77  self._target_id = target_id
78 
79  self._full_chain = FullChainRobotParams(self.sensor_id, self.sensor_id+"_cb_link")
80 
82 
83  def update_config(self, robot_params):
84  self._full_chain.update_config(robot_params)
85  self._checkerboard = robot_params.checkerboards[ self._target_id ]
86 
87  def compute_residual(self, target_pts):
88  h_mat = self.compute_expected(target_pts)
89  z_mat = self.get_measurement()
90  assert(h_mat.shape == z_mat.shape)
91  assert(h_mat.shape[0] == 4)
92  r_mat = h_mat[0:3,:] - z_mat[0:3,:]
93  r = array(reshape(r_mat.T, [-1,1]))[:,0]
94  return r
95 
96  def compute_residual_scaled(self, target_pts):
97  r = self.compute_residual(target_pts)
98  gamma_sqrt = self.compute_marginal_gamma_sqrt(target_pts)
99  r_scaled = gamma_sqrt * matrix(r).T
100  return array(r_scaled.T)[0]
101 
102  def compute_marginal_gamma_sqrt(self, target_pts):
103  import scipy.linalg
104  # ----- Populate Here -----
105  cov = self.compute_cov(target_pts)
106  gamma = matrix(zeros(cov.shape))
107  num_pts = self.get_residual_length()/3
108 
109  for k in range(num_pts):
110  #print "k=%u" % k
111  first = 3*k
112  last = 3*k+3
113  sub_cov = matrix(cov[first:last, first:last])
114  sub_gamma_sqrt_full = matrix(scipy.linalg.sqrtm(sub_cov.I))
115  sub_gamma_sqrt = real(sub_gamma_sqrt_full)
116  assert(scipy.linalg.norm(sub_gamma_sqrt_full - sub_gamma_sqrt) < 1e-6)
117  gamma[first:last, first:last] = sub_gamma_sqrt
118  return gamma
119 
120  def compute_cov(self, target_pts):
121  epsilon = 1e-8
122 
123  num_joints = len(self._M_chain.chain_state.position)
124  Jt = zeros([num_joints, self.get_residual_length()])
125 
126  x = JointState()
127  x.position = self._M_chain.chain_state.position[:]
128 
129  f0 = reshape(array(self._calc_fk_target_pts(x)[0:3,:].T), [-1])
130  for i in range(num_joints):
131  x.position = list(self._M_chain.chain_state.position[:])
132  x.position[i] += epsilon
133  fTest = reshape(array(self._calc_fk_target_pts(x)[0:3,:].T), [-1])
134  Jt[i] = (fTest - f0)/epsilon
135  cov_angles = [x*x for x in self._full_chain.calc_block._chain._cov_dict['joint_angles']]
136  cov = matrix(Jt).T * matrix(diag(cov_angles)) * matrix(Jt)
137 
138  if ( self._full_chain.calc_block._chain._cov_dict.has_key('translation') ):
139  translation_var = self._full_chain.calc_block._chain._cov_dict['translation'];
140  translation_cov = numpy.diag(translation_var*(self.get_residual_length()/3))
141  cov = cov + translation_cov
142 
143  return cov
144 
146  pts = self._checkerboard.generate_points()
147  N = pts.shape[1]
148  return N*3
149 
150  def get_measurement(self):
151  '''
152  Returns a 4xN matrix with the locations of the checkerboard points in homogenous coords,
153  as per the forward kinematics of the chain
154  '''
155  return self._calc_fk_target_pts(self._M_chain.chain_state)
156 
157  def _calc_fk_target_pts(self, chain_state):
158  # Get the target's model points in the frame of the tip of the target chain
159  target_pts_tip = self._checkerboard.generate_points()
160 
161  # Target pose in root frame
162  target_pose_root = self._full_chain.calc_block.fk(chain_state)
163 
164  # Transform points into the root frame
165  target_pts_root = target_pose_root * target_pts_tip
166 
167  return target_pts_root
168 
169  def compute_expected(self, target_pts):
170  return target_pts
171 
172  # Build a dictionary that defines which parameters will in fact affect this measurement
174  sparsity = self._full_chain.build_sparsity_dict()
175  sparsity['checkerboards'] = {}
176  sparsity['checkerboards'][self._target_id] = { 'spacing_x': 1, 'spacing_y': 1 }
177  return sparsity
178 
def __init__(self, config_dict, M_chain, target_id)
Definition: chain_sensor.py:70


calibration_estimation
Author(s): Vijay Pradeep, Michael Ferguson
autogenerated on Thu Jun 6 2019 19:17:16