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/mat.hpp>
6 #include <opencv2/imgproc/imgproc.hpp>
7 #include <stdexcept>
8 #include <string>
9 #include <math.h>
10 #include "exports.h"
11 
12 namespace image_geometry {
13 
14 class Exception : public std::runtime_error
15 {
16 public:
17  Exception(const std::string& description) : std::runtime_error(description) {}
18 };
19 
25 {
26 public:
27 
29 
31 
32  PinholeCameraModel& operator=(const PinholeCameraModel& other);
33 
37  bool fromCameraInfo(const sensor_msgs::CameraInfo& msg);
38 
42  bool fromCameraInfo(const sensor_msgs::CameraInfoConstPtr& msg);
43 
47  std::string tfFrame() const;
48 
52  ros::Time stamp() const;
53 
60  cv::Size fullResolution() const;
61 
69  cv::Size reducedResolution() const;
70 
71  cv::Point2d toFullResolution(const cv::Point2d& uv_reduced) const;
72 
73  cv::Rect toFullResolution(const cv::Rect& roi_reduced) const;
74 
75  cv::Point2d toReducedResolution(const cv::Point2d& uv_full) const;
76 
77  cv::Rect toReducedResolution(const cv::Rect& roi_full) const;
78 
82  cv::Rect rawRoi() const;
83 
87  cv::Rect rectifiedRoi() const;
88 
97  cv::Point2d project3dToPixel(const cv::Point3d& xyz) const;
98 
110  cv::Point3d projectPixelTo3dRay(const cv::Point2d& uv_rect) const;
111  cv::Point3d projectPixelTo3dRay(const cv::Point2d& uv_rect, const cv::Matx34d& P) const;
112 
116  void rectifyImage(const cv::Mat& raw, cv::Mat& rectified,
117  int interpolation = cv::INTER_LINEAR) const;
118 
122  void unrectifyImage(const cv::Mat& rectified, cv::Mat& raw,
123  int interpolation = cv::INTER_LINEAR) const;
124 
128  cv::Point2d rectifyPoint(const cv::Point2d& uv_raw) const;
129  cv::Point2d rectifyPoint(const cv::Point2d& uv_raw, const cv::Matx33d& K, const cv::Matx34d& P) const;
130 
134  cv::Point2d unrectifyPoint(const cv::Point2d& uv_rect) const;
135  cv::Point2d unrectifyPoint(const cv::Point2d& uv_rect, const cv::Matx33d& K, const cv::Matx34d& P) const;
136 
140  cv::Rect rectifyRoi(const cv::Rect& roi_raw) const;
141 
145  cv::Rect unrectifyRoi(const cv::Rect& roi_rect) const;
146 
150  const sensor_msgs::CameraInfo& cameraInfo() const;
151 
155  const cv::Matx33d& intrinsicMatrix() const;
156 
160  const cv::Mat_<double>& distortionCoeffs() const;
161 
165  const cv::Matx33d& rotationMatrix() const;
166 
170  const cv::Matx34d& projectionMatrix() const;
171 
175  const cv::Matx33d& fullIntrinsicMatrix() const;
176 
180  const cv::Matx34d& fullProjectionMatrix() const;
181 
185  double fx() const;
186 
190  double fy() const;
191 
195  double cx() const;
196 
200  double cy() const;
201 
205  double Tx() const;
206 
210  double Ty() const;
211 
215  double fovX() const;
216 
220  double fovY() const;
221 
225  uint32_t binningX() const;
226 
230  uint32_t binningY() const;
231 
240  double getDeltaU(double deltaX, double Z) const;
241 
250  double getDeltaV(double deltaY, double Z) const;
251 
260  double getDeltaX(double deltaU, double Z) const;
261 
270  double getDeltaY(double deltaV, double Z) const;
271 
275  bool initialized() const { return (bool)cache_; }
276 
277 protected:
278  sensor_msgs::CameraInfo cam_info_;
279  cv::Mat_<double> D_; // Unaffected by binning, ROI
280  cv::Matx33d R_; // Unaffected by binning, ROI
281  cv::Matx33d K_; // Describe current image (includes binning, ROI)
282  cv::Matx34d P_; // Describe current image (includes binning, ROI)
283  cv::Matx33d K_full_; // Describe full-res image, needed for full maps
284  cv::Matx34d P_full_; // Describe full-res image, needed for full maps
285 
286  // Use PIMPL here so we can change internals in patch updates if needed
287  struct Cache;
288 #ifdef BOOST_SHARED_PTR_HPP_INCLUDED
289  boost::shared_ptr<Cache> cache_; // Holds cached data for internal use
290 #else
291  std::shared_ptr<Cache> cache_; // Holds cached data for internal use
292 #endif
293 
294  void initRectificationMaps() const;
295  void initUnrectificationMaps() const;
296 
297  friend class StereoCameraModel;
298 };
299 
300 
301 /* Trivial inline functions */
302 inline std::string PinholeCameraModel::tfFrame() const
303 {
304  assert( initialized() );
305  return cam_info_.header.frame_id;
306 }
307 
309 {
310  assert( initialized() );
311  return cam_info_.header.stamp;
312 }
313 
314 inline const sensor_msgs::CameraInfo& PinholeCameraModel::cameraInfo() const { return cam_info_; }
315 inline const cv::Matx33d& PinholeCameraModel::intrinsicMatrix() const { return K_; }
316 inline const cv::Mat_<double>& PinholeCameraModel::distortionCoeffs() const { return D_; }
317 inline const cv::Matx33d& PinholeCameraModel::rotationMatrix() const { return R_; }
318 inline const cv::Matx34d& PinholeCameraModel::projectionMatrix() const { return P_; }
319 inline const cv::Matx33d& PinholeCameraModel::fullIntrinsicMatrix() const { return K_full_; }
320 inline const cv::Matx34d& PinholeCameraModel::fullProjectionMatrix() const { return P_full_; }
321 
322 inline double PinholeCameraModel::fx() const { return P_(0,0); }
323 inline double PinholeCameraModel::fy() const { return P_(1,1); }
324 inline double PinholeCameraModel::cx() const { return P_(0,2); }
325 inline double PinholeCameraModel::cy() const { return P_(1,2); }
326 inline double PinholeCameraModel::Tx() const { return P_(0,3); }
327 inline double PinholeCameraModel::Ty() const { return P_(1,3); }
328 
329 inline double PinholeCameraModel::fovX() const {
330  return 2 * atan(rawRoi().width / (2 * fx()));
331 }
332 inline double PinholeCameraModel::fovY() const {
333  return 2 * atan(rawRoi().height / (2 * fy()));
334 }
335 
336 inline uint32_t PinholeCameraModel::binningX() const { return cam_info_.binning_x; }
337 inline uint32_t PinholeCameraModel::binningY() const { return cam_info_.binning_y; }
338 
339 inline double PinholeCameraModel::getDeltaU(double deltaX, double Z) const
340 {
341  assert( initialized() );
342  return fx() * deltaX / Z;
343 }
344 
345 inline double PinholeCameraModel::getDeltaV(double deltaY, double Z) const
346 {
347  assert( initialized() );
348  return fy() * deltaY / Z;
349 }
350 
351 inline double PinholeCameraModel::getDeltaX(double deltaU, double Z) const
352 {
353  assert( initialized() );
354  return Z * deltaU / fx();
355 }
356 
357 inline double PinholeCameraModel::getDeltaY(double deltaV, double Z) const
358 {
359  assert( initialized() );
360  return Z * deltaV / fy();
361 }
362 
363 } //namespace image_geometry
364 
365 #endif
image_geometry::PinholeCameraModel::D_
cv::Mat_< double > D_
Definition: pinhole_camera_model.h:279
image_geometry::PinholeCameraModel::Ty
double Ty() const
Returns the y-translation term of the projection matrix.
Definition: pinhole_camera_model.h:327
image_geometry::PinholeCameraModel::binningX
uint32_t binningX() const
Returns the number of columns in each bin.
Definition: pinhole_camera_model.h:336
image_geometry::Exception
Definition: pinhole_camera_model.h:14
boost::shared_ptr
image_geometry::PinholeCameraModel::intrinsicMatrix
const cv::Matx33d & intrinsicMatrix() const
Returns the original camera matrix.
Definition: pinhole_camera_model.h:315
image_geometry::PinholeCameraModel::cam_info_
sensor_msgs::CameraInfo cam_info_
Definition: pinhole_camera_model.h:278
image_geometry::PinholeCameraModel::fovY
double fovY() const
Returns the vertical field of view in radians.
Definition: pinhole_camera_model.h:332
image_geometry::PinholeCameraModel::binningY
uint32_t binningY() const
Returns the number of rows in each bin.
Definition: pinhole_camera_model.h:337
image_geometry::PinholeCameraModel::fovX
double fovX() const
Returns the horizontal field of view in radians.
Definition: pinhole_camera_model.h:329
image_geometry::PinholeCameraModel
Simplifies interpreting images geometrically using the parameters from sensor_msgs/CameraInfo.
Definition: pinhole_camera_model.h:24
image_geometry::PinholeCameraModel::fullIntrinsicMatrix
const cv::Matx33d & fullIntrinsicMatrix() const
Returns the original camera matrix for full resolution.
Definition: pinhole_camera_model.h:319
image_geometry
Definition: pinhole_camera_model.h:12
image_geometry::PinholeCameraModel::tfFrame
std::string tfFrame() const
Get the name of the camera coordinate frame in tf.
Definition: pinhole_camera_model.h:302
image_geometry::PinholeCameraModel::getDeltaX
double getDeltaX(double deltaU, double Z) const
Compute delta X, given Z in Cartesian space and delta u in pixels.
Definition: pinhole_camera_model.h:351
image_geometry::Exception::Exception
Exception(const std::string &description)
Definition: pinhole_camera_model.h:17
image_geometry::PinholeCameraModel::cameraInfo
const sensor_msgs::CameraInfo & cameraInfo() const
Returns the camera info message held internally.
Definition: pinhole_camera_model.h:314
image_geometry::PinholeCameraModel::getDeltaU
double getDeltaU(double deltaX, double Z) const
Compute delta u, given Z and delta X in Cartesian space.
Definition: pinhole_camera_model.h:339
image_geometry::PinholeCameraModel::Tx
double Tx() const
Returns the x-translation term of the projection matrix.
Definition: pinhole_camera_model.h:326
image_geometry::PinholeCameraModel::P_full_
cv::Matx34d P_full_
Definition: pinhole_camera_model.h:284
image_geometry::PinholeCameraModel::getDeltaV
double getDeltaV(double deltaY, double Z) const
Compute delta v, given Z and delta Y in Cartesian space.
Definition: pinhole_camera_model.h:345
image_geometry::PinholeCameraModel::fx
double fx() const
Returns the focal length (pixels) in x direction of the rectified image.
Definition: pinhole_camera_model.h:322
image_geometry::PinholeCameraModel::cache_
std::shared_ptr< Cache > cache_
Definition: pinhole_camera_model.h:287
image_geometry::PinholeCameraModel::stamp
ros::Time stamp() const
Get the time stamp associated with this camera model.
Definition: pinhole_camera_model.h:308
exports.h
image_geometry::StereoCameraModel
Simplifies interpreting stereo image pairs geometrically using the parameters from the left and right...
Definition: stereo_camera_model.h:13
image_geometry::PinholeCameraModel::getDeltaY
double getDeltaY(double deltaV, double Z) const
Compute delta Y, given Z in Cartesian space and delta v in pixels.
Definition: pinhole_camera_model.h:357
image_geometry::PinholeCameraModel::fullProjectionMatrix
const cv::Matx34d & fullProjectionMatrix() const
Returns the projection matrix for full resolution.
Definition: pinhole_camera_model.h:320
image_geometry::PinholeCameraModel::cy
double cy() const
Returns the y coordinate of the optical center.
Definition: pinhole_camera_model.h:325
image_geometry::PinholeCameraModel::R_
cv::Matx33d R_
Definition: pinhole_camera_model.h:280
image_geometry::PinholeCameraModel::rotationMatrix
const cv::Matx33d & rotationMatrix() const
Returns the rotation matrix.
Definition: pinhole_camera_model.h:317
image_geometry::PinholeCameraModel::K_full_
cv::Matx33d K_full_
Definition: pinhole_camera_model.h:283
IMAGE_GEOMETRY_DECL
#define IMAGE_GEOMETRY_DECL
Definition: exports.h:15
ros::Time
image_geometry::PinholeCameraModel::Cache
Definition: pinhole_camera_model.cpp:13
std
image_geometry::PinholeCameraModel::K_
cv::Matx33d K_
Definition: pinhole_camera_model.h:281
image_geometry::PinholeCameraModel::fy
double fy() const
Returns the focal length (pixels) in y direction of the rectified image.
Definition: pinhole_camera_model.h:323
image_geometry::PinholeCameraModel::cx
double cx() const
Returns the x coordinate of the optical center.
Definition: pinhole_camera_model.h:324
image_geometry::PinholeCameraModel::distortionCoeffs
const cv::Mat_< double > & distortionCoeffs() const
Returns the distortion coefficients.
Definition: pinhole_camera_model.h:316
image_geometry::PinholeCameraModel::initialized
bool initialized() const
Returns true if the camera has been initialized.
Definition: pinhole_camera_model.h:275
image_geometry::PinholeCameraModel::rawRoi
cv::Rect rawRoi() const
The current raw ROI, as used for capture by the camera driver.
Definition: pinhole_camera_model.cpp:276
image_geometry::PinholeCameraModel::projectionMatrix
const cv::Matx34d & projectionMatrix() const
Returns the projection matrix.
Definition: pinhole_camera_model.h:318
image_geometry::PinholeCameraModel::P_
cv::Matx34d P_
Definition: pinhole_camera_model.h:282


image_geometry
Author(s): Patrick Mihelich
autogenerated on Mon Oct 3 2022 02:45:58