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
00033 bool fromCameraInfo(const sensor_msgs::CameraInfo& msg);
00034
00038 bool fromCameraInfo(const sensor_msgs::CameraInfoConstPtr& msg);
00039
00043 std::string tfFrame() const;
00044
00048 ros::Time stamp() const;
00049
00056 cv::Size fullResolution() const;
00057
00065 cv::Size reducedResolution() const;
00066
00067 cv::Point2d toFullResolution(const cv::Point2d& uv_reduced) const;
00068
00069 cv::Rect toFullResolution(const cv::Rect& roi_reduced) const;
00070
00071 cv::Point2d toReducedResolution(const cv::Point2d& uv_full) const;
00072
00073 cv::Rect toReducedResolution(const cv::Rect& roi_full) const;
00074
00078 cv::Rect rawRoi() const;
00079
00083 cv::Rect rectifiedRoi() const;
00084
00086 void project3dToPixel(const cv::Point3d& xyz, cv::Point2d& uv_rect) const ROS_DEPRECATED;
00087
00096 cv::Point2d project3dToPixel(const cv::Point3d& xyz) const;
00097
00098 void projectPixelTo3dRay(const cv::Point2d& uv_rect, cv::Point3d& ray) const ROS_DEPRECATED;
00099
00111 cv::Point3d projectPixelTo3dRay(const cv::Point2d& uv_rect) const;
00112
00116 void rectifyImage(const cv::Mat& raw, cv::Mat& rectified,
00117 int interpolation = CV_INTER_LINEAR) const;
00118
00122 void unrectifyImage(const cv::Mat& rectified, cv::Mat& raw,
00123 int interpolation = CV_INTER_LINEAR) const;
00124
00125 void rectifyPoint(const cv::Point2d& uv_raw, cv::Point2d& uv_rect) const ROS_DEPRECATED;
00126
00130 cv::Point2d rectifyPoint(const cv::Point2d& uv_raw) const;
00131
00132 void unrectifyPoint(const cv::Point2d& uv_rect, cv::Point2d& uv_raw) const ROS_DEPRECATED;
00133
00137 cv::Point2d unrectifyPoint(const cv::Point2d& uv_rect) const;
00138
00142 cv::Rect rectifyRoi(const cv::Rect& roi_raw) const;
00143
00147 cv::Rect unrectifyRoi(const cv::Rect& roi_rect) const;
00148
00152 const cv::Mat_<double>& intrinsicMatrix() const;
00153
00157 const cv::Mat_<double>& distortionCoeffs() const;
00158
00162 const cv::Mat_<double>& rotationMatrix() const;
00163
00167 const cv::Mat_<double>& projectionMatrix() const;
00168
00172 const cv::Mat_<double>& fullIntrinsicMatrix() const;
00173
00177 const cv::Mat_<double>& fullProjectionMatrix() const;
00178
00182 double fx() const;
00183
00187 double fy() const;
00188
00192 double cx() const;
00193
00197 double cy() const;
00198
00202 double Tx() const;
00203
00207 double Ty() const;
00208
00212 uint32_t height() const ROS_DEPRECATED;
00213
00217 uint32_t width() const ROS_DEPRECATED;
00218
00222 uint32_t binningX() const;
00223
00227 uint32_t binningY() const;
00228
00237 double getDeltaU(double deltaX, double Z) const;
00238
00247 double getDeltaV(double deltaY, double Z) const;
00248
00257 double getDeltaX(double deltaU, double Z) const;
00258
00267 double getDeltaY(double deltaV, double Z) const;
00268
00269 protected:
00270 sensor_msgs::CameraInfo cam_info_;
00271 cv::Mat_<double> D_, R_;
00272 cv::Mat_<double> K_, P_;
00273 cv::Mat_<double> K_full_, P_full_;
00274
00275
00276 struct Cache;
00277 boost::shared_ptr<Cache> cache_;
00278
00279 void initRectificationMaps() const;
00280
00281 bool initialized() const { return cache_; }
00282
00283 friend class StereoCameraModel;
00284 };
00285
00286
00287
00288 inline std::string PinholeCameraModel::tfFrame() const
00289 {
00290 assert( initialized() );
00291 return cam_info_.header.frame_id;
00292 }
00293
00294 inline ros::Time PinholeCameraModel::stamp() const
00295 {
00296 assert( initialized() );
00297 return cam_info_.header.stamp;
00298 }
00299
00300 inline const cv::Mat_<double>& PinholeCameraModel::intrinsicMatrix() const { return K_; }
00301 inline const cv::Mat_<double>& PinholeCameraModel::distortionCoeffs() const { return D_; }
00302 inline const cv::Mat_<double>& PinholeCameraModel::rotationMatrix() const { return R_; }
00303 inline const cv::Mat_<double>& PinholeCameraModel::projectionMatrix() const { return P_; }
00304 inline const cv::Mat_<double>& PinholeCameraModel::fullIntrinsicMatrix() const { return K_full_; }
00305 inline const cv::Mat_<double>& PinholeCameraModel::fullProjectionMatrix() const { return P_full_; }
00306
00307 inline double PinholeCameraModel::fx() const { return P_(0,0); }
00308 inline double PinholeCameraModel::fy() const { return P_(1,1); }
00309 inline double PinholeCameraModel::cx() const { return P_(0,2); }
00310 inline double PinholeCameraModel::cy() const { return P_(1,2); }
00311 inline double PinholeCameraModel::Tx() const { return P_(0,3); }
00312 inline double PinholeCameraModel::Ty() const { return P_(1,3); }
00313 inline uint32_t PinholeCameraModel::height() const { return cam_info_.height; }
00314 inline uint32_t PinholeCameraModel::width() const { return cam_info_.width; }
00315
00316 inline uint32_t PinholeCameraModel::binningX() const { return cam_info_.binning_x; }
00317 inline uint32_t PinholeCameraModel::binningY() const { return cam_info_.binning_y; }
00318
00319 inline double PinholeCameraModel::getDeltaU(double deltaX, double Z) const
00320 {
00321 assert( initialized() );
00322 return fx() * deltaX / Z;
00323 }
00324
00325 inline double PinholeCameraModel::getDeltaV(double deltaY, double Z) const
00326 {
00327 assert( initialized() );
00328 return fy() * deltaY / Z;
00329 }
00330
00331 inline double PinholeCameraModel::getDeltaX(double deltaU, double Z) const
00332 {
00333 assert( initialized() );
00334 return Z * deltaU / fx();
00335 }
00336
00337 inline double PinholeCameraModel::getDeltaY(double deltaV, double Z) const
00338 {
00339 assert( initialized() );
00340 return Z * deltaV / fy();
00341 }
00342
00343 }
00344
00345 #endif