CameraModel.h
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #ifndef CAMERAMODEL_H_
00029 #define CAMERAMODEL_H_
00030 
00031 #include <opencv2/opencv.hpp>
00032 
00033 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
00034 #include "rtabmap/core/Transform.h"
00035 
00036 namespace rtabmap {
00037 
00038 class RTABMAP_EXP CameraModel
00039 {
00040 public:
00041         CameraModel();
00042         // K is the camera intrinsic 3x3 CV_64FC1
00043         // D is the distortion coefficients 1x5 CV_64FC1
00044         // R is the rectification matrix 3x3 CV_64FC1 (computed from stereo or Identity)
00045         // P is the projection matrix 3x4 CV_64FC1 (computed from stereo or equal to [K [0 0 1]'])
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_;} //intrinsic camera matrix
00066         const cv::Mat & D() const {return D_;} //intrinsic distorsion matrix
00067         const cv::Mat & R() const {return R_;} //rectification matrix
00068         const cv::Mat & P() const {return P_;} //projection matrix
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         // For depth images, your should use cv::INTER_NEAREST
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_;} //extrinsic rotation matrix
00122         const cv::Mat & T() const {return T_;} //extrinsic translation matrix
00123         const cv::Mat & E() const {return E_;} //extrinsic essential matrix
00124         const cv::Mat & F() const {return F_;} //extrinsic fundamental matrix
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 } /* namespace rtabmap */
00142 #endif /* CAMERAMODEL_H_ */


rtabmap
Author(s): Mathieu Labbe
autogenerated on Fri Aug 28 2015 12:51:31