camera.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 import numpy
34 from numpy import matrix, array, vsplit, sin, cos, reshape, ones, sqrt
35 import rospy
36 
37 param_names = ['baseline_shift', 'f_shift', 'cx_shift', 'cy_shift']
38 
40  '''
41  Primitive for projecting 3D points into a monocular camera. The baseline
42  term of the projection matrix can be changed. The baseline shift is added
43  to the P[0,3] term of the projection matrix found in CameraInfo
44  Parameters are ordered as follows [baseline_shift, f_shift, cx_shift, cy_shift]
45  '''
46  def __init__(self, config):
47  rospy.logdebug('Initializing rectified camera')
48  self._config = config
49  self._cov_dict = config['cov']
50  try:
51  self._rgbd = config["baseline_rgbd"] != 0.0
52  except:
53  self._rgbd = None
54 
55  def calc_free(self, free_config):
56  return [free_config[x] == 1 for x in param_names]
57 
58  def params_to_config(self, param_vec):
59  param_list = array(param_vec)[:,0].tolist()
60  param_dict = dict(zip(param_names, param_list))
61  param_dict['cov'] = self._cov_dict
62  if self._rgbd:
63  param_dict['baseline_rgbd'] = self._rgbd
64  param_dict['frame_id'] = self._config['frame_id']
65  param_dict['chain_id'] = self._config['chain_id'] # TODO: kill this
66  return param_dict
67 
68  # Convert column vector of params into config, expects a 4x1 matrix
69  def inflate(self, param_vec):
70  param_list = array(param_vec)[:,0].tolist()
71  for x,y in zip(param_names, param_list):
72  self._config[x] = y
73 
74  # Return column vector of config. In this case, it's always a 4x1 matrix
75  def deflate(self):
76  return matrix([self._config[x] for x in param_names]).T
77 
78  # Returns # of params needed for inflation & deflation
79  def get_length(self):
80  return len(param_names)
81 
82  def get_param_names(self):
83  return param_names
84 
85  # Project a set of 3D points points into pixel coordinates
86  # P_list - Projection matrix. We expect this to be a 1x12 list. We then reshape
87  # it into a 3x4 matrix (by filling 1 row at a time)
88  # pts - 4xN numpy matrix holding the points that we want to project (homogenous coords)
89  def project(self, P_list, pts):
90  N = pts.shape[1]
91 
92  # Reshape P_list into an actual matrix
93  P = reshape( matrix(P_list, float), (3,4) )
94 
95  # Update the baseline by the "baseline_shift"
96  P[0,3] = P[0,3] + self._config['baseline_shift']
97  P[0,0] = P[0,0] + self._config['f_shift']
98  P[1,1] = P[1,1] + self._config['f_shift']
99  P[0,2] = P[0,2] + self._config['cx_shift']
100  P[1,2] = P[1,2] + self._config['cy_shift']
101 
102  if (pts.shape[0] == 3):
103  rospy.logfatal('Got vector of points with only 3 rows. Was expecting at 4 rows (homogenous coordinates)')
104 
105  # Apply projection matrix
106  pixel_pts_h = P * pts
107 
108  # Strip out last row (3rd) and rescale
109  if self._rgbd:
110  pixel_pts = pixel_pts_h[0:3,:] / pixel_pts_h[2,:]
111  for i in range(N):
112  depth = pts[2,i]
113  pixel_pts[2,i] = self.get_disparity(P,depth)
114  else:
115  pixel_pts = pixel_pts_h[0:2,:] / pixel_pts_h[2,:]
116 
117  return pixel_pts
118 
119  def get_disparity(self, P_list, depth):
120  P = reshape( matrix(P_list, float), (3,4) )
121  return (P[0,0] * self._config["baseline_rgbd"]) / depth
122 
123 
def get_disparity(self, P_list, depth)
Definition: camera.py:119


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