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 "exports.h"
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  cv::Point3d projectPixelTo3dRay(const cv::Point2d& uv_rect, const cv::Matx34d& P) const;
111 
115  void rectifyImage(const cv::Mat& raw, cv::Mat& rectified,
116  int interpolation = cv::INTER_LINEAR) const;
117 
121  void unrectifyImage(const cv::Mat& rectified, cv::Mat& raw,
122  int interpolation = cv::INTER_LINEAR) const;
123 
127  cv::Point2d rectifyPoint(const cv::Point2d& uv_raw) const;
128  cv::Point2d rectifyPoint(const cv::Point2d& uv_raw, const cv::Matx33d& K, const cv::Matx34d& P) const;
129 
133  cv::Point2d unrectifyPoint(const cv::Point2d& uv_rect) const;
134  cv::Point2d unrectifyPoint(const cv::Point2d& uv_rect, const cv::Matx33d& K, const cv::Matx34d& P) const;
135 
139  cv::Rect rectifyRoi(const cv::Rect& roi_raw) const;
140 
144  cv::Rect unrectifyRoi(const cv::Rect& roi_rect) const;
145 
149  const sensor_msgs::CameraInfo& cameraInfo() const;
150 
154  const cv::Matx33d& intrinsicMatrix() const;
155 
159  const cv::Mat_<double>& distortionCoeffs() const;
160 
164  const cv::Matx33d& rotationMatrix() const;
165 
169  const cv::Matx34d& projectionMatrix() const;
170 
174  const cv::Matx33d& fullIntrinsicMatrix() const;
175 
179  const cv::Matx34d& fullProjectionMatrix() const;
180 
184  double fx() const;
185 
189  double fy() const;
190 
194  double cx() const;
195 
199  double cy() const;
200 
204  double Tx() const;
205 
209  double Ty() const;
210 
214  uint32_t binningX() const;
215 
219  uint32_t binningY() const;
220 
229  double getDeltaU(double deltaX, double Z) const;
230 
239  double getDeltaV(double deltaY, double Z) const;
240 
249  double getDeltaX(double deltaU, double Z) const;
250 
259  double getDeltaY(double deltaV, double Z) const;
260 
264  bool initialized() const { return (bool)cache_; }
265 
266 protected:
267  sensor_msgs::CameraInfo cam_info_;
268  cv::Mat_<double> D_; // Unaffected by binning, ROI
269  cv::Matx33d R_; // Unaffected by binning, ROI
270  cv::Matx33d K_; // Describe current image (includes binning, ROI)
271  cv::Matx34d P_; // Describe current image (includes binning, ROI)
272  cv::Matx33d K_full_; // Describe full-res image, needed for full maps
273  cv::Matx34d P_full_; // Describe full-res image, needed for full maps
274 
275  // Use PIMPL here so we can change internals in patch updates if needed
276  struct Cache;
277 #ifdef BOOST_SHARED_PTR_HPP_INCLUDED
278  boost::shared_ptr<Cache> cache_; // Holds cached data for internal use
279 #else
280  std::shared_ptr<Cache> cache_; // Holds cached data for internal use
281 #endif
282 
283  void initRectificationMaps() const;
284  void initUnrectificationMaps() const;
285 
286  friend class StereoCameraModel;
287 };
288 
289 
290 /* Trivial inline functions */
291 inline std::string PinholeCameraModel::tfFrame() const
292 {
293  assert( initialized() );
294  return cam_info_.header.frame_id;
295 }
296 
298 {
299  assert( initialized() );
300  return cam_info_.header.stamp;
301 }
302 
303 inline const sensor_msgs::CameraInfo& PinholeCameraModel::cameraInfo() const { return cam_info_; }
304 inline const cv::Matx33d& PinholeCameraModel::intrinsicMatrix() const { return K_; }
305 inline const cv::Mat_<double>& PinholeCameraModel::distortionCoeffs() const { return D_; }
306 inline const cv::Matx33d& PinholeCameraModel::rotationMatrix() const { return R_; }
307 inline const cv::Matx34d& PinholeCameraModel::projectionMatrix() const { return P_; }
308 inline const cv::Matx33d& PinholeCameraModel::fullIntrinsicMatrix() const { return K_full_; }
309 inline const cv::Matx34d& PinholeCameraModel::fullProjectionMatrix() const { return P_full_; }
310 
311 inline double PinholeCameraModel::fx() const { return P_(0,0); }
312 inline double PinholeCameraModel::fy() const { return P_(1,1); }
313 inline double PinholeCameraModel::cx() const { return P_(0,2); }
314 inline double PinholeCameraModel::cy() const { return P_(1,2); }
315 inline double PinholeCameraModel::Tx() const { return P_(0,3); }
316 inline double PinholeCameraModel::Ty() const { return P_(1,3); }
317 
318 inline uint32_t PinholeCameraModel::binningX() const { return cam_info_.binning_x; }
319 inline uint32_t PinholeCameraModel::binningY() const { return cam_info_.binning_y; }
320 
321 inline double PinholeCameraModel::getDeltaU(double deltaX, double Z) const
322 {
323  assert( initialized() );
324  return fx() * deltaX / Z;
325 }
326 
327 inline double PinholeCameraModel::getDeltaV(double deltaY, double Z) const
328 {
329  assert( initialized() );
330  return fy() * deltaY / Z;
331 }
332 
333 inline double PinholeCameraModel::getDeltaX(double deltaU, double Z) const
334 {
335  assert( initialized() );
336  return Z * deltaU / fx();
337 }
338 
339 inline double PinholeCameraModel::getDeltaY(double deltaV, double Z) const
340 {
341  assert( initialized() );
342  return Z * deltaV / fy();
343 }
344 
345 } //namespace image_geometry
346 
347 #endif
image_geometry::PinholeCameraModel::D_
cv::Mat_< double > D_
Definition: pinhole_camera_model.h:268
image_geometry::PinholeCameraModel::Ty
double Ty() const
Returns the y-translation term of the projection matrix.
Definition: pinhole_camera_model.h:316
image_geometry::PinholeCameraModel::binningX
uint32_t binningX() const
Returns the number of columns in each bin.
Definition: pinhole_camera_model.h:318
image_geometry::Exception
Definition: pinhole_camera_model.h:13
boost::shared_ptr
image_geometry::PinholeCameraModel::intrinsicMatrix
const cv::Matx33d & intrinsicMatrix() const
Returns the original camera matrix.
Definition: pinhole_camera_model.h:304
image_geometry::PinholeCameraModel::cam_info_
sensor_msgs::CameraInfo cam_info_
Definition: pinhole_camera_model.h:267
image_geometry::PinholeCameraModel::binningY
uint32_t binningY() const
Returns the number of rows in each bin.
Definition: pinhole_camera_model.h:319
image_geometry::PinholeCameraModel
Simplifies interpreting images geometrically using the parameters from sensor_msgs/CameraInfo.
Definition: pinhole_camera_model.h:23
image_geometry::PinholeCameraModel::fullIntrinsicMatrix
const cv::Matx33d & fullIntrinsicMatrix() const
Returns the original camera matrix for full resolution.
Definition: pinhole_camera_model.h:308
image_geometry
Definition: pinhole_camera_model.h:11
image_geometry::PinholeCameraModel::tfFrame
std::string tfFrame() const
Get the name of the camera coordinate frame in tf.
Definition: pinhole_camera_model.h:291
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:333
image_geometry::Exception::Exception
Exception(const std::string &description)
Definition: pinhole_camera_model.h:16
image_geometry::PinholeCameraModel::cameraInfo
const sensor_msgs::CameraInfo & cameraInfo() const
Returns the camera info message held internally.
Definition: pinhole_camera_model.h:303
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:321
image_geometry::PinholeCameraModel::Tx
double Tx() const
Returns the x-translation term of the projection matrix.
Definition: pinhole_camera_model.h:315
image_geometry::PinholeCameraModel::P_full_
cv::Matx34d P_full_
Definition: pinhole_camera_model.h:273
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:327
image_geometry::PinholeCameraModel::fx
double fx() const
Returns the focal length (pixels) in x direction of the rectified image.
Definition: pinhole_camera_model.h:311
image_geometry::PinholeCameraModel::cache_
std::shared_ptr< Cache > cache_
Definition: pinhole_camera_model.h:276
image_geometry::PinholeCameraModel::stamp
ros::Time stamp() const
Get the time stamp associated with this camera model.
Definition: pinhole_camera_model.h:297
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:339
image_geometry::PinholeCameraModel::fullProjectionMatrix
const cv::Matx34d & fullProjectionMatrix() const
Returns the projection matrix for full resolution.
Definition: pinhole_camera_model.h:309
image_geometry::PinholeCameraModel::cy
double cy() const
Returns the y coordinate of the optical center.
Definition: pinhole_camera_model.h:314
image_geometry::PinholeCameraModel::R_
cv::Matx33d R_
Definition: pinhole_camera_model.h:269
image_geometry::PinholeCameraModel::rotationMatrix
const cv::Matx33d & rotationMatrix() const
Returns the rotation matrix.
Definition: pinhole_camera_model.h:306
image_geometry::PinholeCameraModel::K_full_
cv::Matx33d K_full_
Definition: pinhole_camera_model.h:272
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:270
image_geometry::PinholeCameraModel::fy
double fy() const
Returns the focal length (pixels) in y direction of the rectified image.
Definition: pinhole_camera_model.h:312
image_geometry::PinholeCameraModel::cx
double cx() const
Returns the x coordinate of the optical center.
Definition: pinhole_camera_model.h:313
image_geometry::PinholeCameraModel::distortionCoeffs
const cv::Mat_< double > & distortionCoeffs() const
Returns the distortion coefficients.
Definition: pinhole_camera_model.h:305
image_geometry::PinholeCameraModel::initialized
bool initialized() const
Returns true if the camera has been initialized.
Definition: pinhole_camera_model.h:264
image_geometry::PinholeCameraModel::projectionMatrix
const cv::Matx34d & projectionMatrix() const
Returns the projection matrix.
Definition: pinhole_camera_model.h:307
image_geometry::PinholeCameraModel::P_
cv::Matx34d P_
Definition: pinhole_camera_model.h:271


image_geometry
Author(s): Patrick Mihelich
autogenerated on Wed Mar 2 2022 01:12:39