multi_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 from calibration_estimation.sensors import tilting_laser_sensor, chain_sensor, camera_chain_sensor
36 from numpy import concatenate
37 from numpy import zeros, cumsum, matrix, array
38 
39 def block_diag(m_list):
40  '''
41  Given a list of matricies. Combine into a larger, block diagonal matrix. This really should
42  exist in numpy
43  '''
44  # Must be square
45  for m in m_list:
46  assert(m.shape[0] == m.shape[1])
47  m_sizes = [m.shape[0] for m in m_list ]
48  end_ind = list(cumsum(m_sizes))
49  start_ind = [0] + end_ind[0:-1]
50  result = zeros( [end_ind[-1], end_ind[-1] ] )
51  for first, last, m in zip(start_ind, end_ind, m_list):
52  result[first:last, first:last] = m
53  return matrix(result)
54 
56  '''
57  Provides helper methods for dealing with all the sensor measurements
58  generated from a single RobotMeasurement/CbPose pair
59  '''
60  def __init__(self, sensor_configs):
61  self._sensor_configs = sensor_configs
62  self.sensors = []
63  self.checkerboard = "NONE"
64 
65  def sensors_from_message(self, msg):
66  sensors = []
67 
68  sensor_type = 'tilting_lasers'
69  if sensor_type in self._sensor_configs.keys():
70  cur_bundler = tilting_laser_sensor.TiltingLaserBundler( self._sensor_configs[sensor_type] )
71  cur_sensors = cur_bundler.build_blocks(msg)
72  sensors.extend(cur_sensors)
73  else:
74  print "[%s] section doesn't exist. Skipping" % sensor_type
75 
76  sensor_type = 'chains'
77  if sensor_type in self._sensor_configs.keys():
78  cur_bundler = chain_sensor.ChainBundler( self._sensor_configs[sensor_type] )
79  cur_sensors = cur_bundler.build_blocks(msg)
80  sensors.extend(cur_sensors)
81  else:
82  print "[%s] section doesn't exist. Skipping" % sensor_type
83 
84  sensor_type = 'rectified_cams'
85  if sensor_type in self._sensor_configs.keys():
86  cur_bundler = camera_chain_sensor.CameraChainBundler( self._sensor_configs[sensor_type] )
87  cur_sensors = cur_bundler.build_blocks(msg)
88  sensors.extend(cur_sensors)
89  else:
90  print "[%s] section doesn't exist. Skipping" % sensor_type
91 
92  # Store the sensor list internally
93  self.sensors = sensors
94  self.checkerboard = msg.target_id
95 
96  def update_config(self, robot_params):
97  for sensor in self.sensors:
98  sensor.update_config(robot_params)
99 
100  def compute_residual(self, target_pts):
101  r_list = [sensor.compute_residual(target_pts) for sensor in self.sensors]
102  if len(r_list) == 0:
103  return array([])
104 
105  r = concatenate(r_list,0)
106  return r
107 
108  def compute_residual_scaled(self, target_pts):
109  r_list = [sensor.compute_residual_scaled(target_pts) for sensor in self.sensors]
110  if len(r_list) == 0:
111  return array([])
112 
113  r = concatenate(r_list,0)
114  return r
115 
116  def compute_marginal_gamma_sqrt(self, target_pts):
117  gamma_sqrt_list = [sensor.compute_marginal_gamma_sqrt(target_pts) for sensor in self.sensors]
118  if len(gamma_sqrt_list) == 0:
119  return matrix([])
120  gamma_sqrt = block_diag(gamma_sqrt_list)
121  return gamma_sqrt
122 
124  return sum([sensor.get_residual_length() for sensor in self.sensors])
125 


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