Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #ifndef CAMERAMODEL_H_
00029 #define CAMERAMODEL_H_
00030
00031 #include <opencv2/opencv.hpp>
00032
00033 #include "rtabmap/core/RtabmapExp.h"
00034 #include "rtabmap/core/Transform.h"
00035
00036 namespace rtabmap {
00037
00038 class RTABMAP_EXP CameraModel
00039 {
00040 public:
00041 CameraModel();
00042
00043
00044
00045
00046 CameraModel(const std::string & name, const cv::Size & imageSize, const cv::Mat & K, const cv::Mat & D, const cv::Mat & R, const cv::Mat & P);
00047 virtual ~CameraModel() {}
00048
00049 bool isValid() const {return !K_.empty() &&
00050 !D_.empty() &&
00051 !R_.empty() &&
00052 !P_.empty() &&
00053 imageSize_.height &&
00054 imageSize_.width &&
00055 !name_.empty();}
00056
00057 const std::string & name() const {return name_;}
00058
00059 double fx() const {return P_.at<double>(0,0);}
00060 double fy() const {return P_.at<double>(1,1);}
00061 double cx() const {return P_.at<double>(0,2);}
00062 double cy() const {return P_.at<double>(1,2);}
00063 double Tx() const {return P_.at<double>(0,3);}
00064
00065 const cv::Mat & K() const {return K_;}
00066 const cv::Mat & D() const {return D_;}
00067 const cv::Mat & R() const {return R_;}
00068 const cv::Mat & P() const {return P_;}
00069
00070 const cv::Size & imageSize() const {return imageSize_;}
00071 int imageWidth() const {return imageSize_.width;}
00072 int imageWeight() const {return imageSize_.height;}
00073
00074 bool load(const std::string & filePath);
00075 bool save(const std::string & filePath);
00076
00077
00078 cv::Mat rectifyImage(const cv::Mat & raw, int interpolation = cv::INTER_LINEAR) const;
00079 cv::Mat rectifyDepth(const cv::Mat & raw) const;
00080
00081 private:
00082 std::string name_;
00083 cv::Size imageSize_;
00084 cv::Mat K_;
00085 cv::Mat D_;
00086 cv::Mat R_;
00087 cv::Mat P_;
00088 cv::Mat mapX_;
00089 cv::Mat mapY_;
00090 };
00091
00092 class RTABMAP_EXP StereoCameraModel
00093 {
00094 public:
00095 StereoCameraModel() {}
00096 StereoCameraModel(const std::string & name,
00097 const cv::Size & imageSize1,
00098 const cv::Mat & K1, const cv::Mat & D1, const cv::Mat & R1, const cv::Mat & P1,
00099 const cv::Size & imageSize2,
00100 const cv::Mat & K2, const cv::Mat & D2, const cv::Mat & R2, const cv::Mat & P2,
00101 const cv::Mat & R, const cv::Mat & T, const cv::Mat & E, const cv::Mat & F) :
00102 left_(name+"_left", imageSize1, K1, D1, R1, P1),
00103 right_(name+"_right", imageSize2, K2, D2, R2, P2),
00104 name_(name),
00105 R_(R),
00106 T_(T),
00107 E_(E),
00108 F_(F)
00109 {
00110 }
00111 virtual ~StereoCameraModel() {}
00112
00113 bool isValid() const {return left_.isValid() && right_.isValid() && !R_.empty() && !T_.empty() && !E_.empty() && !F_.empty();}
00114 const std::string & name() const {return name_;}
00115
00116 bool load(const std::string & directory, const std::string & cameraName);
00117 bool save(const std::string & directory, const std::string & cameraName);
00118
00119 double baseline() const {return -right_.Tx()/right_.fx();}
00120
00121 const cv::Mat & R() const {return R_;}
00122 const cv::Mat & T() const {return T_;}
00123 const cv::Mat & E() const {return E_;}
00124 const cv::Mat & F() const {return F_;}
00125
00126 Transform transform() const;
00127
00128 const CameraModel & left() const {return left_;}
00129 const CameraModel & right() const {return right_;}
00130
00131 private:
00132 CameraModel left_;
00133 CameraModel right_;
00134 std::string name_;
00135 cv::Mat R_;
00136 cv::Mat T_;
00137 cv::Mat E_;
00138 cv::Mat F_;
00139 };
00140
00141 }
00142 #endif