multi_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 from calibration_estimation.sensors import tilting_laser_sensor, chain_sensor, camera_chain_sensor
00036 from numpy import concatenate
00037 from numpy import zeros, cumsum, matrix, array
00038 
00039 def block_diag(m_list):
00040     '''
00041     Given a list of matricies.  Combine into a larger, block diagonal matrix. This really should
00042     exist in numpy
00043     '''
00044     # Must be square
00045     for m in m_list:
00046         assert(m.shape[0] == m.shape[1])
00047     m_sizes = [m.shape[0] for m in m_list ]
00048     end_ind   = list(cumsum(m_sizes))
00049     start_ind = [0] + end_ind[0:-1]
00050     result = zeros( [end_ind[-1], end_ind[-1] ] )
00051     for first, last, m in zip(start_ind, end_ind, m_list):
00052         result[first:last, first:last] = m
00053     return matrix(result)
00054 
00055 class MultiSensor:
00056     '''
00057     Provides helper methods for dealing with all the sensor measurements
00058     generated from a single RobotMeasurement/CbPose pair
00059     '''
00060     def __init__(self, sensor_configs):
00061         self._sensor_configs = sensor_configs
00062         self.sensors = []
00063         self.checkerboard = "NONE"
00064 
00065     def sensors_from_message(self, msg):
00066         sensors = []
00067 
00068         sensor_type = 'tilting_lasers'
00069         if sensor_type in self._sensor_configs.keys():
00070             cur_bundler = tilting_laser_sensor.TiltingLaserBundler( self._sensor_configs[sensor_type] )
00071             cur_sensors = cur_bundler.build_blocks(msg)
00072             sensors.extend(cur_sensors)
00073         else:
00074             print "[%s] section doesn't exist. Skipping" % sensor_type
00075 
00076         sensor_type = 'chains'
00077         if sensor_type in self._sensor_configs.keys():
00078             cur_bundler = chain_sensor.ChainBundler( self._sensor_configs[sensor_type] )
00079             cur_sensors = cur_bundler.build_blocks(msg)
00080             sensors.extend(cur_sensors)
00081         else:
00082             print "[%s] section doesn't exist. Skipping" % sensor_type
00083 
00084         sensor_type = 'rectified_cams'
00085         if sensor_type in self._sensor_configs.keys():
00086             cur_bundler = camera_chain_sensor.CameraChainBundler( self._sensor_configs[sensor_type] )
00087             cur_sensors = cur_bundler.build_blocks(msg)
00088             sensors.extend(cur_sensors)
00089         else:
00090             print "[%s] section doesn't exist. Skipping" % sensor_type
00091 
00092         # Store the sensor list internally
00093         self.sensors = sensors
00094         self.checkerboard = msg.target_id
00095 
00096     def update_config(self, robot_params):
00097         for sensor in self.sensors:
00098             sensor.update_config(robot_params)
00099 
00100     def compute_residual(self, target_pts):
00101         r_list = [sensor.compute_residual(target_pts) for sensor in self.sensors]
00102         if len(r_list) == 0:
00103             return array([])
00104 
00105         r = concatenate(r_list,0)
00106         return r
00107 
00108     def compute_residual_scaled(self, target_pts):
00109         r_list = [sensor.compute_residual_scaled(target_pts) for sensor in self.sensors]
00110         if len(r_list) == 0:
00111             return array([])
00112 
00113         r = concatenate(r_list,0)
00114         return r
00115 
00116     def compute_marginal_gamma_sqrt(self, target_pts):
00117         gamma_sqrt_list = [sensor.compute_marginal_gamma_sqrt(target_pts) for sensor in self.sensors]
00118         if len(gamma_sqrt_list) == 0:
00119             return matrix([])
00120         gamma_sqrt = block_diag(gamma_sqrt_list)
00121         return gamma_sqrt
00122 
00123     def get_residual_length(self):
00124         return sum([sensor.get_residual_length() for sensor in self.sensors])
00125 


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