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::Mat_<double>& intrinsicMatrix() const;
00151 
00155   const cv::Mat_<double>& distortionCoeffs() const;
00156 
00160   const cv::Mat_<double>& rotationMatrix() const;
00161 
00165   const cv::Mat_<double>& projectionMatrix() const;
00166 
00170   const cv::Mat_<double>& fullIntrinsicMatrix() const;
00171 
00175   const cv::Mat_<double>& 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_, R_;           // Unaffected by binning, ROI
00265   cv::Mat_<double> K_, P_;           // Describe current image (includes binning, ROI)
00266   cv::Mat_<double> K_full_, P_full_; // Describe full-res image, needed for full maps
00267 
00268   // Use PIMPL here so we can change internals in patch updates if needed
00269   struct Cache;
00270   boost::shared_ptr<Cache> cache_; // Holds cached data for internal use
00271 
00272   void initRectificationMaps() const;
00273 
00274   friend class StereoCameraModel;
00275 };
00276 
00277 
00278 /* Trivial inline functions */
00279 inline std::string PinholeCameraModel::tfFrame() const
00280 {
00281   assert( initialized() );
00282   return cam_info_.header.frame_id;
00283 }
00284 
00285 inline ros::Time PinholeCameraModel::stamp() const
00286 {
00287   assert( initialized() );
00288   return cam_info_.header.stamp;
00289 }
00290 
00291 inline const sensor_msgs::CameraInfo& PinholeCameraModel::cameraInfo() const  { return cam_info_; }
00292 inline const cv::Mat_<double>& PinholeCameraModel::intrinsicMatrix() const  { return K_; }
00293 inline const cv::Mat_<double>& PinholeCameraModel::distortionCoeffs() const { return D_; }
00294 inline const cv::Mat_<double>& PinholeCameraModel::rotationMatrix() const   { return R_; }
00295 inline const cv::Mat_<double>& PinholeCameraModel::projectionMatrix() const { return P_; }
00296 inline const cv::Mat_<double>& PinholeCameraModel::fullIntrinsicMatrix() const  { return K_full_; }
00297 inline const cv::Mat_<double>& PinholeCameraModel::fullProjectionMatrix() const { return P_full_; }
00298 
00299 inline double PinholeCameraModel::fx() const { return P_(0,0); }
00300 inline double PinholeCameraModel::fy() const { return P_(1,1); }
00301 inline double PinholeCameraModel::cx() const { return P_(0,2); }
00302 inline double PinholeCameraModel::cy() const { return P_(1,2); }
00303 inline double PinholeCameraModel::Tx() const { return P_(0,3); }
00304 inline double PinholeCameraModel::Ty() const { return P_(1,3); }
00305 
00306 inline uint32_t PinholeCameraModel::binningX() const { return cam_info_.binning_x; }
00307 inline uint32_t PinholeCameraModel::binningY() const { return cam_info_.binning_y; }
00308 
00309 inline double PinholeCameraModel::getDeltaU(double deltaX, double Z) const
00310 {
00311   assert( initialized() );
00312   return fx() * deltaX / Z;
00313 }
00314 
00315 inline double PinholeCameraModel::getDeltaV(double deltaY, double Z) const
00316 {
00317   assert( initialized() );
00318   return fy() * deltaY / Z;
00319 }
00320 
00321 inline double PinholeCameraModel::getDeltaX(double deltaU, double Z) const
00322 {
00323   assert( initialized() );
00324   return Z * deltaU / fx();
00325 }
00326 
00327 inline double PinholeCameraModel::getDeltaY(double deltaV, double Z) const
00328 {
00329   assert( initialized() );
00330   return Z * deltaV / fy();
00331 }
00332 
00333 } //namespace image_geometry
00334 
00335 #endif


image_geometry
Author(s): Patrick Mihelich
autogenerated on Mon Oct 6 2014 08:38:13