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_;
00265 cv::Mat_<double> K_, P_;
00266 cv::Mat_<double> K_full_, P_full_;
00267
00268
00269 struct Cache;
00270 boost::shared_ptr<Cache> cache_;
00271
00272 void initRectificationMaps() const;
00273
00274 friend class StereoCameraModel;
00275 };
00276
00277
00278
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 }
00334
00335 #endif