pinhole_camera_model.h
Go to the documentation of this file.
1 #ifndef IMAGE_GEOMETRY_PINHOLE_CAMERA_MODEL_H
2 #define IMAGE_GEOMETRY_PINHOLE_CAMERA_MODEL_H
3 
4 #include <sensor_msgs/CameraInfo.h>
5 #include <opencv2/core/core.hpp>
6 #include <opencv2/imgproc/imgproc.hpp>
7 #include <opencv2/calib3d/calib3d.hpp>
8 #include <stdexcept>
9 #include <string>
10 
11 namespace image_geometry {
12 
13 class Exception : public std::runtime_error
14 {
15 public:
16  Exception(const std::string& description) : std::runtime_error(description) {}
17 };
18 
24 {
25 public:
26 
28 
30 
31  PinholeCameraModel& operator=(const PinholeCameraModel& other);
32 
36  bool fromCameraInfo(const sensor_msgs::CameraInfo& msg);
37 
41  bool fromCameraInfo(const sensor_msgs::CameraInfoConstPtr& msg);
42 
46  std::string tfFrame() const;
47 
51  ros::Time stamp() const;
52 
59  cv::Size fullResolution() const;
60 
68  cv::Size reducedResolution() const;
69 
70  cv::Point2d toFullResolution(const cv::Point2d& uv_reduced) const;
71 
72  cv::Rect toFullResolution(const cv::Rect& roi_reduced) const;
73 
74  cv::Point2d toReducedResolution(const cv::Point2d& uv_full) const;
75 
76  cv::Rect toReducedResolution(const cv::Rect& roi_full) const;
77 
81  cv::Rect rawRoi() const;
82 
86  cv::Rect rectifiedRoi() const;
87 
96  cv::Point2d project3dToPixel(const cv::Point3d& xyz) const;
97 
109  cv::Point3d projectPixelTo3dRay(const cv::Point2d& uv_rect) const;
110 
114  void rectifyImage(const cv::Mat& raw, cv::Mat& rectified,
115  int interpolation = cv::INTER_LINEAR) const;
116 
120  void unrectifyImage(const cv::Mat& rectified, cv::Mat& raw,
121  int interpolation = cv::INTER_LINEAR) const;
122 
126  cv::Point2d rectifyPoint(const cv::Point2d& uv_raw) const;
127 
131  cv::Point2d unrectifyPoint(const cv::Point2d& uv_rect) const;
132 
136  cv::Rect rectifyRoi(const cv::Rect& roi_raw) const;
137 
141  cv::Rect unrectifyRoi(const cv::Rect& roi_rect) const;
142 
146  const sensor_msgs::CameraInfo& cameraInfo() const;
147 
151  const cv::Matx33d& intrinsicMatrix() const;
152 
156  const cv::Mat_<double>& distortionCoeffs() const;
157 
161  const cv::Matx33d& rotationMatrix() const;
162 
166  const cv::Matx34d& projectionMatrix() const;
167 
171  const cv::Matx33d& fullIntrinsicMatrix() const;
172 
176  const cv::Matx34d& fullProjectionMatrix() const;
177 
181  double fx() const;
182 
186  double fy() const;
187 
191  double cx() const;
192 
196  double cy() const;
197 
201  double Tx() const;
202 
206  double Ty() const;
207 
211  uint32_t binningX() const;
212 
216  uint32_t binningY() const;
217 
226  double getDeltaU(double deltaX, double Z) const;
227 
236  double getDeltaV(double deltaY, double Z) const;
237 
246  double getDeltaX(double deltaU, double Z) const;
247 
256  double getDeltaY(double deltaV, double Z) const;
257 
261  bool initialized() const { return (bool)cache_; }
262 
263 protected:
264  sensor_msgs::CameraInfo cam_info_;
265  cv::Mat_<double> D_; // Unaffected by binning, ROI
266  cv::Matx33d R_; // Unaffected by binning, ROI
267  cv::Matx33d K_; // Describe current image (includes binning, ROI)
268  cv::Matx34d P_; // Describe current image (includes binning, ROI)
269  cv::Matx33d K_full_; // Describe full-res image, needed for full maps
270  cv::Matx34d P_full_; // Describe full-res image, needed for full maps
271 
272  // Use PIMPL here so we can change internals in patch updates if needed
273  struct Cache;
274 #ifdef BOOST_SHARED_PTR_HPP_INCLUDED
275  boost::shared_ptr<Cache> cache_; // Holds cached data for internal use
276 #else
277  std::shared_ptr<Cache> cache_; // Holds cached data for internal use
278 #endif
279 
280  void initRectificationMaps() const;
281 
282  friend class StereoCameraModel;
283 };
284 
285 
286 /* Trivial inline functions */
287 inline std::string PinholeCameraModel::tfFrame() const
288 {
289  assert( initialized() );
290  return cam_info_.header.frame_id;
291 }
292 
294 {
295  assert( initialized() );
296  return cam_info_.header.stamp;
297 }
298 
299 inline const sensor_msgs::CameraInfo& PinholeCameraModel::cameraInfo() const { return cam_info_; }
300 inline const cv::Matx33d& PinholeCameraModel::intrinsicMatrix() const { return K_; }
301 inline const cv::Mat_<double>& PinholeCameraModel::distortionCoeffs() const { return D_; }
302 inline const cv::Matx33d& PinholeCameraModel::rotationMatrix() const { return R_; }
303 inline const cv::Matx34d& PinholeCameraModel::projectionMatrix() const { return P_; }
304 inline const cv::Matx33d& PinholeCameraModel::fullIntrinsicMatrix() const { return K_full_; }
305 inline const cv::Matx34d& PinholeCameraModel::fullProjectionMatrix() const { return P_full_; }
306 
307 inline double PinholeCameraModel::fx() const { return P_(0,0); }
308 inline double PinholeCameraModel::fy() const { return P_(1,1); }
309 inline double PinholeCameraModel::cx() const { return P_(0,2); }
310 inline double PinholeCameraModel::cy() const { return P_(1,2); }
311 inline double PinholeCameraModel::Tx() const { return P_(0,3); }
312 inline double PinholeCameraModel::Ty() const { return P_(1,3); }
313 
314 inline uint32_t PinholeCameraModel::binningX() const { return cam_info_.binning_x; }
315 inline uint32_t PinholeCameraModel::binningY() const { return cam_info_.binning_y; }
316 
317 inline double PinholeCameraModel::getDeltaU(double deltaX, double Z) const
318 {
319  assert( initialized() );
320  return fx() * deltaX / Z;
321 }
322 
323 inline double PinholeCameraModel::getDeltaV(double deltaY, double Z) const
324 {
325  assert( initialized() );
326  return fy() * deltaY / Z;
327 }
328 
329 inline double PinholeCameraModel::getDeltaX(double deltaU, double Z) const
330 {
331  assert( initialized() );
332  return Z * deltaU / fx();
333 }
334 
335 inline double PinholeCameraModel::getDeltaY(double deltaV, double Z) const
336 {
337  assert( initialized() );
338  return Z * deltaV / fy();
339 }
340 
341 } //namespace image_geometry
342 
343 #endif
const cv::Matx33d & intrinsicMatrix() const
Returns the original camera matrix.
bool initialized() const
Returns true if the camera has been initialized.
double Ty() const
Returns the y-translation term of the projection matrix.
const sensor_msgs::CameraInfo & cameraInfo() const
Returns the camera info message held internally.
double cx() const
Returns the x coordinate of the optical center.
double getDeltaY(double deltaV, double Z) const
Compute delta Y, given Z in Cartesian space and delta v in pixels.
double cy() const
Returns the y coordinate of the optical center.
uint32_t binningX() const
Returns the number of columns in each bin.
std::string tfFrame() const
Get the name of the camera coordinate frame in tf.
const cv::Matx33d & rotationMatrix() const
Returns the rotation matrix.
double Tx() const
Returns the x-translation term of the projection matrix.
ros::Time stamp() const
Get the time stamp associated with this camera model.
double getDeltaU(double deltaX, double Z) const
Compute delta u, given Z and delta X in Cartesian space.
const cv::Matx34d & fullProjectionMatrix() const
Returns the projection matrix for full resolution.
double getDeltaX(double deltaU, double Z) const
Compute delta X, given Z in Cartesian space and delta u in pixels.
Simplifies interpreting images geometrically using the parameters from sensor_msgs/CameraInfo.
double fy() const
Returns the focal length (pixels) in y direction of the rectified image.
uint32_t binningY() const
Returns the number of rows in each bin.
double getDeltaV(double deltaY, double Z) const
Compute delta v, given Z and delta Y in Cartesian space.
const cv::Matx34d & projectionMatrix() const
Returns the projection matrix.
const cv::Mat_< double > & distortionCoeffs() const
Returns the distortion coefficients.
const cv::Matx33d & fullIntrinsicMatrix() const
Returns the original camera matrix for full resolution.
double fx() const
Returns the focal length (pixels) in x direction of the rectified image.
Exception(const std::string &description)
Simplifies interpreting stereo image pairs geometrically using the parameters from the left and right...


image_geometry
Author(s): Patrick Mihelich
autogenerated on Thu Dec 12 2019 03:52:09