EpipolarGeometry.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 #pragma once
00029 
00030 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
00031 #include "rtabmap/core/Parameters.h"
00032 #include <opencv2/core/core.hpp>
00033 #include <opencv2/features2d/features2d.hpp>
00034 #include <pcl/point_cloud.h>
00035 #include <pcl/point_types.h>
00036 #include <list>
00037 #include <vector>
00038 
00039 namespace rtabmap
00040 {
00041 
00042 class Signature;
00043 
00044 class RTABMAP_EXP EpipolarGeometry
00045 {
00046 public:
00047         EpipolarGeometry(const ParametersMap & parameters = ParametersMap());
00048         virtual ~EpipolarGeometry();
00049         bool check(const Signature * ssA, const Signature * ssB);
00050         void parseParameters(const ParametersMap & parameters);
00051 
00052         int getMatchCountMinAccepted() const {return _matchCountMinAccepted;}
00053         double getRansacParam1() const {return _ransacParam1;}
00054         double getRansacParam2() const {return _ransacParam2;}
00055 
00056         void setMatchCountMinAccepted(int matchCountMinAccepted) {_matchCountMinAccepted = matchCountMinAccepted;}
00057         void setRansacParam1(double ransacParam1) {_ransacParam1 = ransacParam1;}
00058         void setRansacParam2(double ransacParam2) {_ransacParam2 = ransacParam2;}
00059 
00060 
00061         // STATIC STUFF
00062         //epipolar geometry
00063         static void findEpipolesFromF(
00064                         const cv::Mat & fundamentalMatrix,
00065                         cv::Vec3d & e1,
00066                         cv::Vec3d & e2);
00067 
00068         static cv::Mat findPFromE(
00069                         const cv::Mat & E,
00070                         const cv::Mat & x,
00071                         const cv::Mat & xp);
00072 
00073         // return fundamental matrix
00074         // status -> inliers = 1, outliers = 0
00075         static cv::Mat findFFromWords(
00076                         const std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs, // id, kpt1, kpt2
00077                         std::vector<uchar> & status,
00078                         double ransacParam1 = 3.0,
00079                         double ransacParam2 = 0.99);
00080 
00081         // assume a canonical camera (without K)
00082         static void findRTFromP(
00083                         const cv::Mat & p,
00084                         cv::Mat & r,
00085                         cv::Mat & t);
00086 
00087         static cv::Mat findFFromCalibratedStereoCameras(double fx, double fy, double cx, double cy, double Tx, double Ty);
00088 
00093         static int findPairs(
00094                         const std::multimap<int, cv::KeyPoint> & wordsA,
00095                         const std::multimap<int, cv::KeyPoint> & wordsB,
00096                         std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs);
00097 
00102         static int findPairsUnique(
00103                         const std::multimap<int, cv::KeyPoint> & wordsA,
00104                         const std::multimap<int, cv::KeyPoint> & wordsB,
00105                         std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs);
00106 
00111         static int findPairsAll(
00112                         const std::multimap<int, cv::KeyPoint> & wordsA,
00113                         const std::multimap<int, cv::KeyPoint> & wordsB,
00114                         std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs);
00115 
00116         static cv::Mat linearLSTriangulation(
00117                         cv::Point3d u,    //homogenous image point (u,v,1)
00118                         cv::Matx34d P,        //camera 1 matrix 3x4 double
00119                         cv::Point3d u1,   //homogenous image point in 2nd camera
00120                         cv::Matx34d P1);       //camera 2 matrix 3x4 double
00121 
00122         static cv::Mat iterativeLinearLSTriangulation(
00123                         cv::Point3d u,        //homogenous image point (u,v,1)
00124                         const cv::Matx34d & P,    //camera 1 matrix 3x4 double
00125                         cv::Point3d u1,        //homogenous image point in 2nd camera
00126                         const cv::Matx34d & P1);  //camera 2 matrix 3x4 double
00127 
00128         static double triangulatePoints(
00129                         const cv::Mat& pt_set1, //2xN double
00130                         const cv::Mat& pt_set2, //2xN double
00131                         const cv::Mat& P, // 3x4 double
00132                         const cv::Mat& P1, // 3x4 double
00133                         pcl::PointCloud<pcl::PointXYZ>::Ptr & pointcloud,
00134                         std::vector<double> & reproj_errors);
00135 
00136 private:
00137         int _matchCountMinAccepted;
00138         double _ransacParam1;
00139         double _ransacParam2;
00140 };
00141 
00142 } // namespace rtabmap


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