pinhole_camera_model.h

Go to the documentation of this file.

#ifndef IMAGE_GEOMETRY_PINHOLE_CAMERA_MODEL_H
#define IMAGE_GEOMETRY_PINHOLE_CAMERA_MODEL_H

#include <sensor_msgs/CameraInfo.h>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <stdexcept>

namespace image_geometry {

class Exception : public std::runtime_error
{
public:
  Exception(const std::string& description) : std::runtime_error(description) {}
};

class PinholeCameraModel
{
public:
  
  PinholeCameraModel();

  PinholeCameraModel(const PinholeCameraModel& other);

  bool fromCameraInfo(const sensor_msgs::CameraInfo& msg);

  bool fromCameraInfo(const sensor_msgs::CameraInfoConstPtr& msg);

  std::string tfFrame() const;

  ros::Time stamp() const;

  cv::Size fullResolution() const;

  cv::Size reducedResolution() const;

  cv::Point2d toFullResolution(const cv::Point2d& uv_reduced) const;

  cv::Rect toFullResolution(const cv::Rect& roi_reduced) const;

  cv::Point2d toReducedResolution(const cv::Point2d& uv_full) const;

  cv::Rect toReducedResolution(const cv::Rect& roi_full) const;

  cv::Rect rawRoi() const;

  cv::Rect rectifiedRoi() const;

  void project3dToPixel(const cv::Point3d& xyz, cv::Point2d& uv_rect) const ROS_DEPRECATED;
  
  cv::Point2d project3dToPixel(const cv::Point3d& xyz) const;

  void projectPixelTo3dRay(const cv::Point2d& uv_rect, cv::Point3d& ray) const ROS_DEPRECATED;

  cv::Point3d projectPixelTo3dRay(const cv::Point2d& uv_rect) const;

  void rectifyImage(const cv::Mat& raw, cv::Mat& rectified,
                    int interpolation = CV_INTER_LINEAR) const;

  void unrectifyImage(const cv::Mat& rectified, cv::Mat& raw,
                      int interpolation = CV_INTER_LINEAR) const;

  void rectifyPoint(const cv::Point2d& uv_raw, cv::Point2d& uv_rect) const ROS_DEPRECATED;
  
  cv::Point2d rectifyPoint(const cv::Point2d& uv_raw) const;

  void unrectifyPoint(const cv::Point2d& uv_rect, cv::Point2d& uv_raw) const ROS_DEPRECATED;
  
  cv::Point2d unrectifyPoint(const cv::Point2d& uv_rect) const;

  cv::Rect rectifyRoi(const cv::Rect& roi_raw) const;

  cv::Rect unrectifyRoi(const cv::Rect& roi_rect) const;
  
  const cv::Mat_<double>& intrinsicMatrix() const;

  const cv::Mat_<double>& distortionCoeffs() const;

  const cv::Mat_<double>& rotationMatrix() const;

  const cv::Mat_<double>& projectionMatrix() const;

  const cv::Mat_<double>& fullIntrinsicMatrix() const;

  const cv::Mat_<double>& fullProjectionMatrix() const;

  double fx() const;

  double fy() const;

  double cx() const;

  double cy() const;

  double Tx() const;

  double Ty() const;

  uint32_t height() const ROS_DEPRECATED;

  uint32_t width() const ROS_DEPRECATED;

  uint32_t binningX() const;

  uint32_t binningY() const;
  
  double getDeltaU(double deltaX, double Z) const;

  double getDeltaV(double deltaY, double Z) const;

  double getDeltaX(double deltaU, double Z) const;

  double getDeltaY(double deltaV, double Z) const;

  bool initialized() const { return cache_; }

protected:
  sensor_msgs::CameraInfo cam_info_;
  cv::Mat_<double> D_, R_;           // Unaffected by binning, ROI
  cv::Mat_<double> K_, P_;           // Describe current image (includes binning, ROI)
  cv::Mat_<double> K_full_, P_full_; // Describe full-res image, needed for full maps

  // Use PIMPL here so we can change internals in patch updates if needed
  struct Cache;
  boost::shared_ptr<Cache> cache_; // Holds cached data for internal use

  void initRectificationMaps() const;

  friend class StereoCameraModel;
};


/* Trivial inline functions */
inline std::string PinholeCameraModel::tfFrame() const
{
  assert( initialized() );
  return cam_info_.header.frame_id;
}

inline ros::Time PinholeCameraModel::stamp() const
{
  assert( initialized() );
  return cam_info_.header.stamp;
}

inline const cv::Mat_<double>& PinholeCameraModel::intrinsicMatrix() const  { return K_; }
inline const cv::Mat_<double>& PinholeCameraModel::distortionCoeffs() const { return D_; }
inline const cv::Mat_<double>& PinholeCameraModel::rotationMatrix() const   { return R_; }
inline const cv::Mat_<double>& PinholeCameraModel::projectionMatrix() const { return P_; }
inline const cv::Mat_<double>& PinholeCameraModel::fullIntrinsicMatrix() const  { return K_full_; }
inline const cv::Mat_<double>& PinholeCameraModel::fullProjectionMatrix() const { return P_full_; }

inline double PinholeCameraModel::fx() const { return P_(0,0); }
inline double PinholeCameraModel::fy() const { return P_(1,1); }
inline double PinholeCameraModel::cx() const { return P_(0,2); }
inline double PinholeCameraModel::cy() const { return P_(1,2); }
inline double PinholeCameraModel::Tx() const { return P_(0,3); }
inline double PinholeCameraModel::Ty() const { return P_(1,3); }
inline uint32_t PinholeCameraModel::height() const { return cam_info_.height; }
inline uint32_t PinholeCameraModel::width() const  { return cam_info_.width; }

inline uint32_t PinholeCameraModel::binningX() const { return cam_info_.binning_x; }
inline uint32_t PinholeCameraModel::binningY() const { return cam_info_.binning_y; }

inline double PinholeCameraModel::getDeltaU(double deltaX, double Z) const
{
  assert( initialized() );
  return fx() * deltaX / Z;
}

inline double PinholeCameraModel::getDeltaV(double deltaY, double Z) const
{
  assert( initialized() );
  return fy() * deltaY / Z;
}

inline double PinholeCameraModel::getDeltaX(double deltaU, double Z) const
{
  assert( initialized() );
  return Z * deltaU / fx();
}

inline double PinholeCameraModel::getDeltaY(double deltaV, double Z) const
{
  assert( initialized() );
  return Z * deltaV / fy();
}

} //namespace image_geometry

#endif