pinhole_camera_model.h
Go to the documentation of this file.
00001 #ifndef IMAGE_GEOMETRY_PINHOLE_CAMERA_MODEL_H
00002 #define IMAGE_GEOMETRY_PINHOLE_CAMERA_MODEL_H
00003 
00004 #include <sensor_msgs/CameraInfo.h>
00005 #include <opencv2/core/core.hpp>
00006 #include <opencv2/imgproc/imgproc.hpp>
00007 #include <opencv2/calib3d/calib3d.hpp>
00008 #include <stdexcept>
00009 
00010 namespace image_geometry {
00011 
00012 class Exception : public std::runtime_error
00013 {
00014 public:
00015   Exception(const std::string& description) : std::runtime_error(description) {}
00016 };
00017 
00022 class PinholeCameraModel
00023 {
00024 public:
00025   
00026   PinholeCameraModel();
00027 
00028   PinholeCameraModel(const PinholeCameraModel& other);
00029 
00030   PinholeCameraModel& operator=(const PinholeCameraModel& other);
00031 
00035   bool fromCameraInfo(const sensor_msgs::CameraInfo& msg);
00036 
00040   bool fromCameraInfo(const sensor_msgs::CameraInfoConstPtr& msg);
00041 
00045   std::string tfFrame() const;
00046 
00050   ros::Time stamp() const;
00051 
00058   cv::Size fullResolution() const;
00059 
00067   cv::Size reducedResolution() const;
00068 
00069   cv::Point2d toFullResolution(const cv::Point2d& uv_reduced) const;
00070 
00071   cv::Rect toFullResolution(const cv::Rect& roi_reduced) const;
00072 
00073   cv::Point2d toReducedResolution(const cv::Point2d& uv_full) const;
00074 
00075   cv::Rect toReducedResolution(const cv::Rect& roi_full) const;
00076 
00080   cv::Rect rawRoi() const;
00081 
00085   cv::Rect rectifiedRoi() const;
00086 
00095   cv::Point2d project3dToPixel(const cv::Point3d& xyz) const;
00096 
00108   cv::Point3d projectPixelTo3dRay(const cv::Point2d& uv_rect) const;
00109 
00113   void rectifyImage(const cv::Mat& raw, cv::Mat& rectified,
00114                     int interpolation = cv::INTER_LINEAR) const;
00115 
00119   void unrectifyImage(const cv::Mat& rectified, cv::Mat& raw,
00120                       int interpolation = cv::INTER_LINEAR) const;
00121 
00125   cv::Point2d rectifyPoint(const cv::Point2d& uv_raw) const;
00126 
00130   cv::Point2d unrectifyPoint(const cv::Point2d& uv_rect) const;
00131 
00135   cv::Rect rectifyRoi(const cv::Rect& roi_raw) const;
00136 
00140   cv::Rect unrectifyRoi(const cv::Rect& roi_rect) const;
00141 
00145   const sensor_msgs::CameraInfo& cameraInfo() const;
00146 
00150   const cv::Matx33d& intrinsicMatrix() const;
00151 
00155   const cv::Mat_<double>& distortionCoeffs() const;
00156 
00160   const cv::Matx33d& rotationMatrix() const;
00161 
00165   const cv::Matx34d& projectionMatrix() const;
00166 
00170   const cv::Matx33d& fullIntrinsicMatrix() const;
00171 
00175   const cv::Matx34d& fullProjectionMatrix() const;
00176 
00180   double fx() const;
00181 
00185   double fy() const;
00186 
00190   double cx() const;
00191 
00195   double cy() const;
00196 
00200   double Tx() const;
00201 
00205   double Ty() const;
00206 
00210   uint32_t binningX() const;
00211 
00215   uint32_t binningY() const;
00216   
00225   double getDeltaU(double deltaX, double Z) const;
00226 
00235   double getDeltaV(double deltaY, double Z) const;
00236 
00245   double getDeltaX(double deltaU, double Z) const;
00246 
00255   double getDeltaY(double deltaV, double Z) const;
00256 
00260   bool initialized() const { return (bool)cache_; }
00261 
00262 protected:
00263   sensor_msgs::CameraInfo cam_info_;
00264   cv::Mat_<double> D_;           // Unaffected by binning, ROI
00265   cv::Matx33d R_;           // Unaffected by binning, ROI
00266   cv::Matx33d K_;           // Describe current image (includes binning, ROI)
00267   cv::Matx34d P_;           // Describe current image (includes binning, ROI)
00268   cv::Matx33d K_full_; // Describe full-res image, needed for full maps
00269   cv::Matx34d P_full_; // Describe full-res image, needed for full maps
00270 
00271   // Use PIMPL here so we can change internals in patch updates if needed
00272   struct Cache;
00273   boost::shared_ptr<Cache> cache_; // Holds cached data for internal use
00274 
00275   void initRectificationMaps() const;
00276 
00277   friend class StereoCameraModel;
00278 };
00279 
00280 
00281 /* Trivial inline functions */
00282 inline std::string PinholeCameraModel::tfFrame() const
00283 {
00284   assert( initialized() );
00285   return cam_info_.header.frame_id;
00286 }
00287 
00288 inline ros::Time PinholeCameraModel::stamp() const
00289 {
00290   assert( initialized() );
00291   return cam_info_.header.stamp;
00292 }
00293 
00294 inline const sensor_msgs::CameraInfo& PinholeCameraModel::cameraInfo() const  { return cam_info_; }
00295 inline const cv::Matx33d& PinholeCameraModel::intrinsicMatrix() const  { return K_; }
00296 inline const cv::Mat_<double>& PinholeCameraModel::distortionCoeffs() const { return D_; }
00297 inline const cv::Matx33d& PinholeCameraModel::rotationMatrix() const   { return R_; }
00298 inline const cv::Matx34d& PinholeCameraModel::projectionMatrix() const { return P_; }
00299 inline const cv::Matx33d& PinholeCameraModel::fullIntrinsicMatrix() const  { return K_full_; }
00300 inline const cv::Matx34d& PinholeCameraModel::fullProjectionMatrix() const { return P_full_; }
00301 
00302 inline double PinholeCameraModel::fx() const { return P_(0,0); }
00303 inline double PinholeCameraModel::fy() const { return P_(1,1); }
00304 inline double PinholeCameraModel::cx() const { return P_(0,2); }
00305 inline double PinholeCameraModel::cy() const { return P_(1,2); }
00306 inline double PinholeCameraModel::Tx() const { return P_(0,3); }
00307 inline double PinholeCameraModel::Ty() const { return P_(1,3); }
00308 
00309 inline uint32_t PinholeCameraModel::binningX() const { return cam_info_.binning_x; }
00310 inline uint32_t PinholeCameraModel::binningY() const { return cam_info_.binning_y; }
00311 
00312 inline double PinholeCameraModel::getDeltaU(double deltaX, double Z) const
00313 {
00314   assert( initialized() );
00315   return fx() * deltaX / Z;
00316 }
00317 
00318 inline double PinholeCameraModel::getDeltaV(double deltaY, double Z) const
00319 {
00320   assert( initialized() );
00321   return fy() * deltaY / Z;
00322 }
00323 
00324 inline double PinholeCameraModel::getDeltaX(double deltaU, double Z) const
00325 {
00326   assert( initialized() );
00327   return Z * deltaU / fx();
00328 }
00329 
00330 inline double PinholeCameraModel::getDeltaY(double deltaV, double Z) const
00331 {
00332   assert( initialized() );
00333   return Z * deltaV / fy();
00334 }
00335 
00336 } //namespace image_geometry
00337 
00338 #endif


image_geometry
Author(s): Patrick Mihelich
autogenerated on Thu Jun 6 2019 21:23:35