tilting_laser_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 # checkerboard
40 # /
41 # root
42 # \
43 # tilting_laser
44 
45 from numpy import reshape, array, zeros, matrix, diag, real
46 
47 import roslib; roslib.load_manifest('calibration_estimation')
48 import rospy
49 import numpy
50 
52  def __init__(self, valid_configs):
53  self._valid_configs = valid_configs
54 
55  # Construct a tilitng laser for each laser pair that matches one of the configs
56  def build_blocks(self, M_robot):
57  sensors = []
58  for cur_config in self._valid_configs:
59  if cur_config["sensor_id"] in [ x.laser_id for x in M_robot.M_laser ] :
60  M_laser = [x for x in M_robot.M_laser if cur_config["sensor_id"] == x.laser_id][0]
61  cur_sensor = TiltingLaserSensor(cur_config, M_laser)
62  sensors.append(cur_sensor)
63  else:
64  rospy.logdebug(" Didn't find block")
65  return sensors
66 
68  def __init__(self, config_dict, M_laser):
69  self.sensor_type = "laser"
70  self.sensor_id = config_dict["sensor_id"]
71 
72  self._config_dict = config_dict
73  self._M_laser = M_laser
75 
76  def update_config(self, robot_params):
77  self._tilting_laser = robot_params.tilting_lasers[ self.sensor_id ]
78  self._tilting_laser.update_config(robot_params)
79 
80  def compute_residual(self, target_pts):
81  z_mat = self.get_measurement()
82  h_mat = self.compute_expected(target_pts)
83  assert(z_mat.shape[0] == 4)
84  assert(h_mat.shape[0] == 4)
85  assert(z_mat.shape[1] == z_mat.shape[1])
86  r = array(reshape(h_mat[0:3,:].T - z_mat[0:3,:].T, [-1,1]))[:,0]
87  return r
88 
89  def compute_residual_scaled(self, target_pts):
90  r = self.compute_residual(target_pts)
91  gamma_sqrt = self.compute_marginal_gamma_sqrt(target_pts)
92  r_scaled = gamma_sqrt * matrix(r).T
93  return array(r_scaled.T)[0]
94 
95  def compute_marginal_gamma_sqrt(self, target_pts):
96  import scipy.linalg
97  # ----- Populate Here -----
98  cov = self.compute_cov(target_pts)
99  gamma = matrix(zeros(cov.shape))
100  num_pts = self.get_residual_length()/3
101 
102  for k in range(num_pts):
103  first = 3*k
104  last = 3*k+3
105  sub_cov = matrix(cov[first:last, first:last])
106  sub_gamma_sqrt_full = matrix(scipy.linalg.sqrtm(sub_cov.I))
107  sub_gamma_sqrt = real(sub_gamma_sqrt_full)
108  assert(scipy.linalg.norm(sub_gamma_sqrt_full - sub_gamma_sqrt) < 1e-6)
109  gamma[first:last, first:last] = sub_gamma_sqrt
110  return gamma
111 
113  N = len(self._M_laser.joint_points)
114  return N*3
115 
116  # Get the laser measurement.
117  # Returns a 3xN matrix of points in cartesian space
118  def get_measurement(self):
119  # Get the laser points in a 4xN homogenous matrix
120  laser_pts_root = self._tilting_laser.project_to_3D([x.position for x in self._M_laser.joint_points])
121  return laser_pts_root
122 
123  def compute_cov(self, target_pts):
124  epsilon = 1e-8
125 
126  # Ok, this is coded kind of weird. Jt should be [3*N, len(r)]. Currently, it is
127  # [3, len(r)], which means we are assuming that laser noise is correlated across
128  # multiple points. This is wrong. However, we accidentally fix this when computing
129  # Gamma, so it doesn't show up in the final solution.
130  Jt = zeros([3, self.get_residual_length()])
131 
132  import copy
133  x = [copy.copy(x.position) for x in self._M_laser.joint_points]
134 
135  f0 = reshape(array(self._tilting_laser.project_to_3D(x)[0:3,:].T), [-1])
136  for i in range(3):
137  x = [ [y for y in x.position] for x in self._M_laser.joint_points]
138  for cur_pt in x:
139  cur_pt[i] += epsilon
140  fTest = reshape(array(self._tilting_laser.project_to_3D(x)[0:3,:].T), [-1])
141  Jt[i] = (fTest - f0)/epsilon
142 
143  num_pts = len(x)
144  std_dev_sensor = [self._tilting_laser._cov_dict['tilt'],
145  self._tilting_laser._cov_dict['bearing'],
146  self._tilting_laser._cov_dict['range']]
147  cov_sensor = [x*x for x in std_dev_sensor]
148 
149  cov = matrix(Jt).T * matrix(diag(cov_sensor)) * matrix(Jt)
150  return cov
151 
152  # Returns the pixel coordinates of the laser points after being projected into the camera
153  # Returns a 4xN matrix of the target points
154  def compute_expected(self, target_pts):
155  return target_pts
156 
157  # Build a dictionary that defines which parameters will in fact affect this measurement
159  sparsity = dict()
160 
161  sparsity['tilting_lasers'] = {}
162  sparsity['tilting_lasers'][self.sensor_id] = {'gearing':1}
163  sparsity['transforms'] = {}
164  for cur_transform in ( self._tilting_laser._before_chain_Ts + \
165  self._tilting_laser._after_chain_Ts ):
166  sparsity['transforms'][cur_transform._name] = [1, 1, 1, 1, 1, 1]
167  sparsity['transforms'][self._tilting_laser._config['joint']] = [1, 1, 1, 1, 1, 1]
168 
169  return sparsity
170 


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