camera.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 import numpy
00034 from numpy import matrix, array, vsplit, sin, cos, reshape, ones, sqrt
00035 import rospy
00036 
00037 param_names = ['baseline_shift', 'f_shift', 'cx_shift', 'cy_shift']
00038 
00039 class RectifiedCamera:
00040     '''
00041     Primitive for projecting 3D points into a monocular camera. The baseline
00042     term of the projection matrix can be changed.  The baseline shift is added
00043     to the P[0,3] term of the projection matrix found in CameraInfo
00044     Parameters are ordered as follows [baseline_shift, f_shift, cx_shift, cy_shift]
00045     '''
00046     def __init__(self, config):
00047         rospy.logdebug('Initializing rectified camera')
00048         self._config = config
00049         self._cov_dict = config['cov']
00050         try:
00051             self._rgbd = config["baseline_rgbd"] != 0.0
00052         except:
00053             self._rgbd = None
00054 
00055     def calc_free(self, free_config):
00056         return [free_config[x] == 1 for x in param_names]
00057 
00058     def params_to_config(self, param_vec):
00059         param_list = array(param_vec)[:,0].tolist()
00060         param_dict = dict(zip(param_names, param_list))
00061         param_dict['cov'] = self._cov_dict
00062         if self._rgbd:
00063             param_dict['baseline_rgbd'] = self._rgbd
00064         param_dict['frame_id'] = self._config['frame_id']
00065         param_dict['chain_id'] = self._config['chain_id']   # TODO: kill this
00066         return param_dict
00067 
00068     # Convert column vector of params into config, expects a 4x1 matrix
00069     def inflate(self, param_vec):
00070         param_list = array(param_vec)[:,0].tolist()
00071         for x,y in zip(param_names, param_list):
00072             self._config[x] = y
00073 
00074     # Return column vector of config. In this case, it's always a 4x1 matrix
00075     def deflate(self):
00076         return matrix([self._config[x] for x in param_names]).T
00077 
00078     # Returns # of params needed for inflation & deflation
00079     def get_length(self):
00080         return len(param_names)
00081 
00082     def get_param_names(self):
00083         return param_names
00084 
00085     # Project a set of 3D points points into pixel coordinates
00086     # P_list - Projection matrix. We expect this to be a 1x12 list. We then reshape
00087     #          it into a 3x4 matrix (by filling 1 row at a time)
00088     # pts - 4xN numpy matrix holding the points that we want to project (homogenous coords)
00089     def project(self, P_list, pts):
00090         N = pts.shape[1]
00091 
00092         # Reshape P_list into an actual matrix
00093         P = reshape( matrix(P_list, float), (3,4) )
00094 
00095         # Update the baseline by the "baseline_shift"
00096         P[0,3] = P[0,3] + self._config['baseline_shift']
00097         P[0,0] = P[0,0] + self._config['f_shift']
00098         P[1,1] = P[1,1] + self._config['f_shift']
00099         P[0,2] = P[0,2] + self._config['cx_shift']
00100         P[1,2] = P[1,2] + self._config['cy_shift']
00101 
00102         if (pts.shape[0] == 3):
00103             rospy.logfatal('Got vector of points with only 3 rows. Was expecting at 4 rows (homogenous coordinates)')
00104 
00105         # Apply projection matrix
00106         pixel_pts_h = P * pts
00107 
00108         # Strip out last row (3rd) and rescale
00109         if self._rgbd:
00110             pixel_pts = pixel_pts_h[0:3,:] / pixel_pts_h[2,:]
00111             for i in range(N):
00112                 depth = pts[2,i]
00113                 pixel_pts[2,i] = self.get_disparity(P,depth)
00114         else:
00115             pixel_pts = pixel_pts_h[0:2,:] / pixel_pts_h[2,:]
00116 
00117         return pixel_pts
00118 
00119     def get_disparity(self, P_list, depth):
00120         P = reshape( matrix(P_list, float), (3,4) )
00121         return (P[0,0] * self._config["baseline_rgbd"]) / depth
00122         
00123 


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