camera_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 # Define a camera sensor attached to a chain
36 #
37 # before_chain_Ts -- camera_chain -- after_chain_Ts -- camera
38 # /
39 # root
40 # \
41 # checkerboard
42 
43 import numpy
44 from numpy import matrix, reshape, array, zeros, diag, real
45 
46 import roslib; roslib.load_manifest('calibration_estimation')
47 import rospy
48 from calibration_estimation.full_chain import FullChainRobotParams
49 from sensor_msgs.msg import JointState
50 
52  """
53  Tool used to generate a list of CameraChain sensors from a single calibration sample message
54  """
55  def __init__(self, valid_configs):
56  self._valid_configs = valid_configs
57 
58  # Construct a CameraChainSensor for every camera chain sensor that exists in the given robot measurement
59  def build_blocks(self, M_robot):
60  """
61  Input:
62  - M_robot: A calibration sample of type calibration_msgs/RobotMeasurement
63  Returns:
64  - sensors: A list of CameraChainSensor objects that corresponds to all camera chain sensors found
65  in the calibration message that was passed in.
66  """
67  sensors = []
68  for cur_config in self._valid_configs:
69  if cur_config["sensor_id"] in [ x.camera_id for x in M_robot.M_cam ]:
70  if "chain_id" in cur_config.keys() and cur_config["chain_id"] != None and cur_config["chain_id"] != "NONE":
71  if cur_config["chain_id"] in [ x.chain_id for x in M_robot.M_chain ] :
72  M_cam = M_robot.M_cam [ [ x.camera_id for x in M_robot.M_cam ].index(cur_config["sensor_id"])]
73  M_chain = M_robot.M_chain[ [ x.chain_id for x in M_robot.M_chain ].index(cur_config["chain_id"]) ]
74  if cur_config["frame_id"] != M_cam.cam_info.header.frame_id:
75  rospy.logwarn('WARNING frame_id of cam_info.header.frame_id({}) from bag file'.format(M_cam.cam_info.header.frame_id))
76  rospy.logwarn(' and sensors.rectified_cams.{}.frame_id({}) of system.yaml differ'.format(cur_config["chain_id"],cur_config["frame_id"]))
77  else:
78  continue
79  else:
80  M_cam = M_robot.M_cam [ [ x.camera_id for x in M_robot.M_cam ].index(cur_config["sensor_id"])]
81  M_chain = None
82  cur_config["chain_id"] = None
83  cur_sensor = CameraChainSensor(cur_config, M_cam, M_chain)
84  sensors.append(cur_sensor)
85  else:
86  rospy.logdebug(" Didn't find block")
87  return sensors
88 
90  def __init__(self, config_dict, M_cam, M_chain):
91  """
92  Generates a single sensor block for a single configuration
93  Inputs:
94  - config_dict: The configuration for this specific sensor. This looks exactly like
95  a single element from the valid_configs list passed into CameraChainBundler.__init__
96  - M_cam: The camera measurement of type calibration_msgs/CameraMeasurement
97  - M_chain: The chain measurement of type calibration_msgs/ChainMeasurement
98  """
99  self.sensor_type = "camera"
100  self.sensor_id = config_dict["sensor_id"]
101 
102  self._config_dict = config_dict
103  self._M_cam = M_cam
104  self._M_chain = M_chain
105 
106  self._full_chain = FullChainRobotParams(config_dict["chain_id"], config_dict["frame_id"])
107 
109  if "baseline_rgbd" in config_dict.keys():
110  self.terms_per_sample = 3
111 
112  def update_config(self, robot_params):
113  """
114  On each optimization cycle the set of system parameters that we're optimizing over will change. Thus,
115  after each change in parameters we must call update_config to ensure that our calculated residual reflects
116  the newest set of system parameters.
117  """
118  self._camera = robot_params.rectified_cams[ self.sensor_id ]
119  if self._full_chain is not None:
120  self._full_chain.update_config(robot_params)
121 
122  def compute_residual(self, target_pts):
123  """
124  Computes the measurement residual for the current set of system parameters and target points
125  Input:
126  - target_pts: 4XN matrix, storing features point locations in world cartesian homogenous coordinates.
127  Output:
128  - r: terms_per_sample*N long vector, storing pixel residuals for the target points in the form of
129  [u1, v1, u2, v2, ..., uN, vN] or [u1, v1, u'1, u2....]
130  """
131  z_mat = self.get_measurement()
132  h_mat = self.compute_expected(target_pts)
133  r = array(reshape(h_mat - z_mat, [-1,1]))[:,0]
134  return r
135 
136  def compute_residual_scaled(self, target_pts):
137  """
138  Computes the residual, and then scales it by sqrt(Gamma), where Gamma
139  is the information matrix for this measurement (Cov^-1).
140  """
141  r = self.compute_residual(target_pts)
142  gamma_sqrt = self.compute_marginal_gamma_sqrt(target_pts)
143  r_scaled = gamma_sqrt * matrix(r).T
144  return array(r_scaled.T)[0]
145 
146  def compute_marginal_gamma_sqrt(self, target_pts):
147  """
148  Calculates the square root of the information matrix for the measurement of the
149  current set of system parameters at the passed in set of target points.
150  """
151  import scipy.linalg
152  cov = self.compute_cov(target_pts)
153  gamma = matrix(zeros(cov.shape))
154  num_pts = self.get_residual_length()/self.terms_per_sample
155 
156  for k in range(num_pts):
157  first = self.terms_per_sample*k
158  last = self.terms_per_sample*k+self.terms_per_sample
159  sub_cov = matrix(cov[first:last, first:last])
160  sub_gamma_sqrt_full = matrix(scipy.linalg.sqrtm(sub_cov.I))
161  sub_gamma_sqrt = real(sub_gamma_sqrt_full)
162  assert(scipy.linalg.norm(sub_gamma_sqrt_full - sub_gamma_sqrt) < 1e-6)
163  gamma[first:last, first:last] = sub_gamma_sqrt
164  return gamma
165 
167  return self.terms_per_sample*len(self._M_cam.image_points)
168 
169  def get_measurement(self):
170  """
171  Get the target's pixel coordinates as measured by the actual sensor
172  """
173  if self.terms_per_sample == 2:
174  return numpy.matrix([[pt.x, pt.y] for pt in self._M_cam.image_points])
175  elif self.terms_per_sample == 3:
176  return numpy.matrix([[pt.x, pt.y, self._camera.get_disparity(self._M_cam.cam_info.P, pt.z)] for pt in self._M_cam.image_points])
177 
178  def compute_expected(self, target_pts):
179  """
180  Compute the expected pixel coordinates for a set of target points.
181  target_pts: 4xN matrix, storing feature points of the target, in homogeneous coords
182  Returns: target points projected into pixel coordinates, in a Nx2 matrix
183  """
184  if self._M_chain is not None:
185  return self._compute_expected(self._M_chain.chain_state, target_pts)
186  else:
187  return self._compute_expected(None, target_pts)
188  def _compute_expected(self, chain_state, target_pts):
189  """
190  Compute what measurement we would expect to see, based on the current system parameters
191  and the specified target point locations.
192 
193  Inputs:
194  - chain_state: The joint angles of this sensor's chain of type sensor_msgs/JointState.
195  - target_pts: 4XN matrix, storing features point locations in world cartesian homogenous coordinates.
196 
197  Returns: target points projected into pixel coordinates, in a Nx2 matrix
198  """
199  # Camera pose in root frame
200  camera_pose_root = self._full_chain.calc_block.fk(chain_state)
201  cam_frame_pts = camera_pose_root.I * target_pts
202  # Do the camera projection
203  pixel_pts = self._camera.project(self._M_cam.cam_info.P, cam_frame_pts)
204  return pixel_pts.T
205 
206  def compute_expected_J(self, target_pts):
207  """
208  The output Jacobian J shows how moving target_pts in cartesian space affects
209  the expected measurement in (u,v) camera coordinates.
210  For n points in target_pts, J is a 2nx3n matrix
211  Note: This doesn't seem to be used anywhere, except maybe in some drawing code
212  """
213  epsilon = 1e-8
214  N = len(self._M_cam.image_points)
215  Jt = zeros([N*3, N*self.terms_per_sample])
216  for k in range(N):
217  # Compute jacobian for point k
218  sub_Jt = zeros([3,self.terms_per_sample])
219  x = target_pts[:,k].copy()
220  f0 = self.compute_expected(x)
221  for i in [0,1,2]:
222  x[i,0] += epsilon
223  fTest = self.compute_expected(x)
224  sub_Jt[i,:] = array((fTest - f0) / epsilon)
225  x[i,0] -= epsilon
226  Jt[k*3:(k+1)*3, k*self.terms_per_sample:(k+1)*self.terms_per_sample] = sub_Jt
227  return Jt.T
228 
229  def compute_cov(self, target_pts):
230  '''
231  Computes the measurement covariance in pixel coordinates for the given
232  set of target points (target_pts)
233  Input:
234  - target_pts: 4xN matrix, storing N feature points of the target, in homogeneous coords
235  '''
236  cam_cov = matrix(zeros([self.get_residual_length(), self.get_residual_length()]))
237 
238  # Convert StdDev into variance
239  var_u = self._camera._cov_dict['u'] * self._camera._cov_dict['u']
240  var_v = self._camera._cov_dict['v'] * self._camera._cov_dict['v']
241  for k in range(cam_cov.shape[0]/self.terms_per_sample):
242  cam_cov[self.terms_per_sample*k , self.terms_per_sample*k] = var_u
243  cam_cov[self.terms_per_sample*k+1, self.terms_per_sample*k+1] = var_v
244  if self.terms_per_sample == 3:
245  cam_cov[self.terms_per_sample*k+2, self.terms_per_sample*k+2] = self._camera._cov_dict['x'] * self._camera._cov_dict['x']
246 
247  # Both chain and camera covariances are now in measurement space, so we can simply add them together
248  if self._M_chain is not None:
249  return self.get_chain_cov(target_pts) + cam_cov
250  else:
251  return cam_cov
252 
253  def get_chain_cov(self, target_pts, epsilon=1e-8):
254  num_joints = len(self._M_chain.chain_state.position)
255  Jt = zeros([num_joints, self.get_residual_length()])
256 
257  x = JointState()
258  x.position = self._M_chain.chain_state.position[:]
259 
260  # Compute the Jacobian from the chain's joint angles to pixel residuals
261  f0 = reshape(array(self._compute_expected(x, target_pts)), [-1])
262  for i in range(num_joints):
263  x.position = [cur_pos for cur_pos in self._M_chain.chain_state.position]
264  x.position[i] += epsilon
265  fTest = reshape(array(self._compute_expected(x, target_pts)), [-1])
266  Jt[i] = (fTest - f0)/epsilon
267  cov_angles = [x*x for x in self._full_chain.calc_block._chain._cov_dict['joint_angles']]
268 
269  # Transform the chain's covariance from joint angle space into pixel space using the just calculated jacobian
270  return matrix(Jt).T * matrix(diag(cov_angles)) * matrix(Jt)
271 
273  """
274  Build a dictionary that defines which parameters will in fact affect this measurement.
275 
276  Returned dictionary should be of the following form:
277  transforms:
278  my_transformA: [1, 1, 1, 1, 1, 1]
279  my_transformB: [1, 1, 1, 1, 1, 1]
280  chains:
281  my_chain:
282  gearing: [1, 1, ---, 1]
283  rectified_cams:
284  my_cam:
285  baseline_shift: 1
286  f_shift: 1
287  cx_shift: 1
288  cy_shift: 1
289  """
290  sparsity = self._full_chain.build_sparsity_dict()
291  sparsity['rectified_cams'] = {}
292  sparsity['rectified_cams'][self.sensor_id] = dict( [(x,1) for x in self._camera.get_param_names()] )
293  return sparsity
294 


calibration_estimation
Author(s): Vijay Pradeep, Michael Ferguson
autogenerated on Fri Apr 2 2021 02:12:53