tilting_laser_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 # Error block for a monocular camera plus tilting laser.  The
00036 # camera is attached to a chain, but also has a 6 dof transform
00037 # in between the tilting laser is attached to the
00038 #
00039 #       checkerboard
00040 #      /
00041 #   root
00042 #      \
00043 #       tilting_laser
00044 
00045 from numpy import reshape, array, zeros, matrix, diag, real
00046 
00047 import roslib; roslib.load_manifest('calibration_estimation')
00048 import rospy
00049 import numpy
00050 
00051 class TiltingLaserBundler:
00052     def __init__(self, valid_configs):
00053         self._valid_configs = valid_configs
00054 
00055     # Construct a tilitng laser for each laser pair that matches one of the configs
00056     def build_blocks(self, M_robot):
00057         sensors = []
00058         for cur_config in self._valid_configs:
00059             if cur_config["sensor_id"] in [ x.laser_id  for x in M_robot.M_laser ] :
00060                 M_laser = [x for x in M_robot.M_laser if cur_config["sensor_id"] == x.laser_id][0]
00061                 cur_sensor = TiltingLaserSensor(cur_config, M_laser)
00062                 sensors.append(cur_sensor)
00063             else:
00064                 rospy.logdebug("  Didn't find block")
00065         return sensors
00066 
00067 class TiltingLaserSensor:
00068     def __init__(self, config_dict, M_laser):
00069         self.sensor_type = "laser"
00070         self.sensor_id = config_dict["sensor_id"]
00071 
00072         self._config_dict = config_dict
00073         self._M_laser = M_laser
00074         self.terms_per_sample = 3
00075 
00076     def update_config(self, robot_params):
00077         self._tilting_laser = robot_params.tilting_lasers[ self.sensor_id ]
00078         self._tilting_laser.update_config(robot_params)
00079 
00080     def compute_residual(self, target_pts):
00081         z_mat = self.get_measurement()
00082         h_mat = self.compute_expected(target_pts)
00083         assert(z_mat.shape[0] == 4)
00084         assert(h_mat.shape[0] == 4)
00085         assert(z_mat.shape[1] == z_mat.shape[1])
00086         r = array(reshape(h_mat[0:3,:].T - z_mat[0:3,:].T, [-1,1]))[:,0]
00087         return r
00088 
00089     def compute_residual_scaled(self, target_pts):
00090         r = self.compute_residual(target_pts)
00091         gamma_sqrt = self.compute_marginal_gamma_sqrt(target_pts)
00092         r_scaled = gamma_sqrt * matrix(r).T
00093         return array(r_scaled.T)[0]
00094 
00095     def compute_marginal_gamma_sqrt(self, target_pts):
00096         import scipy.linalg
00097         # ----- Populate Here -----
00098         cov = self.compute_cov(target_pts)
00099         gamma = matrix(zeros(cov.shape))
00100         num_pts = self.get_residual_length()/3
00101 
00102         for k in range(num_pts):
00103             first = 3*k
00104             last = 3*k+3
00105             sub_cov = matrix(cov[first:last, first:last])
00106             sub_gamma_sqrt_full = matrix(scipy.linalg.sqrtm(sub_cov.I))
00107             sub_gamma_sqrt = real(sub_gamma_sqrt_full)
00108             assert(scipy.linalg.norm(sub_gamma_sqrt_full - sub_gamma_sqrt) < 1e-6)
00109             gamma[first:last, first:last] = sub_gamma_sqrt
00110         return gamma
00111 
00112     def get_residual_length(self):
00113         N = len(self._M_laser.joint_points)
00114         return N*3
00115 
00116     # Get the laser measurement.
00117     # Returns a 3xN matrix of points in cartesian space
00118     def get_measurement(self):
00119         # Get the laser points in a 4xN homogenous matrix
00120         laser_pts_root = self._tilting_laser.project_to_3D([x.position for x in self._M_laser.joint_points])
00121         return laser_pts_root
00122 
00123     def compute_cov(self, target_pts):
00124         epsilon = 1e-8
00125 
00126         # Ok, this is coded kind of weird. Jt should be [3*N, len(r)]. Currently, it is
00127         # [3, len(r)], which means we are assuming that laser noise is correlated across
00128         # multiple points. This is wrong.  However, we accidentally fix this when computing
00129         # Gamma, so it doesn't show up in the final solution.
00130         Jt = zeros([3, self.get_residual_length()])
00131 
00132         import copy
00133         x = [copy.copy(x.position) for x in self._M_laser.joint_points]
00134 
00135         f0 = reshape(array(self._tilting_laser.project_to_3D(x)[0:3,:].T), [-1])
00136         for i in range(3):
00137             x = [ [y for y in x.position] for x in self._M_laser.joint_points]
00138             for cur_pt in x:
00139                 cur_pt[i] += epsilon
00140             fTest = reshape(array(self._tilting_laser.project_to_3D(x)[0:3,:].T), [-1])
00141             Jt[i] = (fTest - f0)/epsilon
00142 
00143         num_pts = len(x)
00144         std_dev_sensor = [self._tilting_laser._cov_dict['tilt'],
00145                           self._tilting_laser._cov_dict['bearing'],
00146                           self._tilting_laser._cov_dict['range']]
00147         cov_sensor = [x*x for x in std_dev_sensor]
00148 
00149         cov = matrix(Jt).T * matrix(diag(cov_sensor)) * matrix(Jt)
00150         return cov
00151 
00152     # Returns the pixel coordinates of the laser points after being projected into the camera
00153     # Returns a 4xN matrix of the target points
00154     def compute_expected(self, target_pts):
00155         return target_pts
00156 
00157     # Build a dictionary that defines which parameters will in fact affect this measurement
00158     def build_sparsity_dict(self):
00159         sparsity = dict()
00160 
00161         sparsity['tilting_lasers'] = {}
00162         sparsity['tilting_lasers'][self.sensor_id] = {'gearing':1}
00163         sparsity['transforms'] = {}
00164         for cur_transform in ( self._tilting_laser._before_chain_Ts + \
00165                                self._tilting_laser._after_chain_Ts ): 
00166             sparsity['transforms'][cur_transform._name] = [1, 1, 1, 1, 1, 1]
00167         sparsity['transforms'][self._tilting_laser._config['joint']] = [1, 1, 1, 1, 1, 1]
00168 
00169         return sparsity
00170 


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