EpipolarGeometry.h
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #pragma once
29 
30 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
31 #include "rtabmap/core/Parameters.h"
32 #include "rtabmap/utilite/UStl.h"
33 #include <opencv2/core/core.hpp>
34 #include <opencv2/features2d/features2d.hpp>
35 #include <pcl/point_cloud.h>
36 #include <pcl/point_types.h>
37 #include <list>
38 #include <vector>
39 
40 namespace rtabmap
41 {
42 
43 class Signature;
44 
45 class RTABMAP_EXP EpipolarGeometry
46 {
47 public:
48  EpipolarGeometry(const ParametersMap & parameters = ParametersMap());
49  virtual ~EpipolarGeometry();
50  bool check(const Signature * ssA, const Signature * ssB);
51  void parseParameters(const ParametersMap & parameters);
52 
53  int getMatchCountMinAccepted() const {return _matchCountMinAccepted;}
54  double getRansacParam1() const {return _ransacParam1;}
55  double getRansacParam2() const {return _ransacParam2;}
56 
57  void setMatchCountMinAccepted(int matchCountMinAccepted) {_matchCountMinAccepted = matchCountMinAccepted;}
58  void setRansacParam1(double ransacParam1) {_ransacParam1 = ransacParam1;}
59  void setRansacParam2(double ransacParam2) {_ransacParam2 = ransacParam2;}
60 
61 
62  // STATIC STUFF
63  //epipolar geometry
64  static void findEpipolesFromF(
65  const cv::Mat & fundamentalMatrix,
66  cv::Vec3d & e1,
67  cv::Vec3d & e2);
68 
69  static cv::Mat findPFromE(
70  const cv::Mat & E,
71  const cv::Mat & x,
72  const cv::Mat & xp);
73 
74  // return fundamental matrix
75  // status -> inliers = 1, outliers = 0
76  static cv::Mat findFFromWords(
77  const std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs, // id, kpt1, kpt2
78  std::vector<uchar> & status,
79  double ransacReprojThreshold = 3.0,
80  double ransacConfidence = 0.99);
81 
82  // assume a canonical camera (without K)
83  static void findRTFromP(
84  const cv::Mat & p,
85  cv::Mat & r,
86  cv::Mat & t);
87 
88  static cv::Mat findFFromCalibratedStereoCameras(double fx, double fy, double cx, double cy, double Tx, double Ty);
89 
90 
95  template<typename T>
96  static int findPairs(
97  const std::map<int, T> & wordsA,
98  const std::map<int, T> & wordsB,
99  std::list<std::pair<int, std::pair<T, T> > > & pairs,
100  bool ignoreNegativeIds = true)
101  {
102  int realPairsCount = 0;
103  pairs.clear();
104  for(typename std::map<int, T>::const_iterator i=wordsA.begin(); i!=wordsA.end(); ++i)
105  {
106  if(!ignoreNegativeIds || (ignoreNegativeIds && i->first>=0))
107  {
108  std::map<int, cv::KeyPoint>::const_iterator ptB = wordsB.find(i->first);
109  if(ptB != wordsB.end())
110  {
111  pairs.push_back(std::pair<int, std::pair<T, T> >(i->first, std::make_pair(i->second, ptB->second)));
112  ++realPairsCount;
113  }
114  }
115  }
116  return realPairsCount;
117  }
118 
123  template<typename T>
124  static int findPairs(
125  const std::multimap<int, T> & wordsA,
126  const std::multimap<int, T> & wordsB,
127  std::list<std::pair<int, std::pair<T, T> > > & pairs,
128  bool ignoreNegativeIds = true)
129  {
130  const std::list<int> & ids = uUniqueKeys(wordsA);
131  typename std::multimap<int, T>::const_iterator iterA;
132  typename std::multimap<int, T>::const_iterator iterB;
133  pairs.clear();
134  int realPairsCount = 0;
135  for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
136  {
137  if(!ignoreNegativeIds || (ignoreNegativeIds && *i >= 0))
138  {
139  iterA = wordsA.find(*i);
140  iterB = wordsB.find(*i);
141  while(iterA != wordsA.end() && iterB != wordsB.end() && (*iterA).first == (*iterB).first && (*iterA).first == *i)
142  {
143  pairs.push_back(std::pair<int, std::pair<T, T> >(*i, std::make_pair((*iterA).second, (*iterB).second)));
144  ++iterA;
145  ++iterB;
146  ++realPairsCount;
147  }
148  }
149  }
150  return realPairsCount;
151  }
152 
157  template<typename T>
158  static int findPairsUnique(
159  const std::multimap<int, T> & wordsA,
160  const std::multimap<int, T> & wordsB,
161  std::list<std::pair<int, std::pair<T, T> > > & pairs,
162  bool ignoreNegativeIds = true)
163  {
164  const std::list<int> & ids = uUniqueKeys(wordsA);
165  int realPairsCount = 0;
166  pairs.clear();
167  for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
168  {
169  if(!ignoreNegativeIds || (ignoreNegativeIds && *i>=0))
170  {
171  std::list<T> ptsA = uValues(wordsA, *i);
172  std::list<T> ptsB = uValues(wordsB, *i);
173  if(ptsA.size() == 1 && ptsB.size() == 1)
174  {
175  pairs.push_back(std::pair<int, std::pair<T, T> >(*i, std::pair<T, T>(ptsA.front(), ptsB.front())));
176  ++realPairsCount;
177  }
178  else if(ptsA.size()>1 && ptsB.size()>1)
179  {
180  // just update the count
181  realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
182  }
183  }
184  }
185  return realPairsCount;
186  }
187 
192  template<typename T>
193  static int findPairsAll(
194  const std::multimap<int, T> & wordsA,
195  const std::multimap<int, T> & wordsB,
196  std::list<std::pair<int, std::pair<T, T> > > & pairs,
197  bool ignoreNegativeIds = true)
198  {
199  const std::list<int> & ids = uUniqueKeys(wordsA);
200  pairs.clear();
201  int realPairsCount = 0;;
202  for(std::list<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
203  {
204  if(!ignoreNegativeIds || (ignoreNegativeIds && *iter>=0))
205  {
206  std::list<T> ptsA = uValues(wordsA, *iter);
207  std::list<T> ptsB = uValues(wordsB, *iter);
208 
209  realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
210 
211  for(typename std::list<T>::iterator jter=ptsA.begin(); jter!=ptsA.end(); ++jter)
212  {
213  for(typename std::list<T>::iterator kter=ptsB.begin(); kter!=ptsB.end(); ++kter)
214  {
215  pairs.push_back(std::pair<int, std::pair<T, T> >(*iter, std::pair<T, T>(*jter, *kter)));
216  }
217  }
218  }
219  }
220  return realPairsCount;
221  }
222 
223  static cv::Mat linearLSTriangulation(
224  cv::Point3d u, //homogenous image point (u,v,1)
225  cv::Matx34d P, //camera 1 matrix 3x4 double
226  cv::Point3d u1, //homogenous image point in 2nd camera
227  cv::Matx34d P1); //camera 2 matrix 3x4 double
228 
229  static cv::Mat iterativeLinearLSTriangulation(
230  cv::Point3d u, //homogenous image point (u,v,1)
231  const cv::Matx34d & P, //camera 1 matrix 3x4 double
232  cv::Point3d u1, //homogenous image point in 2nd camera
233  const cv::Matx34d & P1); //camera 2 matrix 3x4 double
234 
235  static double triangulatePoints(
236  const cv::Mat& pt_set1, //2xN double
237  const cv::Mat& pt_set2, //2xN double
238  const cv::Mat& P, // 3x4 double
239  const cv::Mat& P1, // 3x4 double
240  pcl::PointCloud<pcl::PointXYZ>::Ptr & pointcloud,
241  std::vector<double> & reproj_errors);
242 
243 private:
244  int _matchCountMinAccepted;
245  double _ransacParam1;
246  double _ransacParam2;
247 };
248 
249 } // namespace rtabmap
uUniqueKeys
std::list< K > uUniqueKeys(const std::multimap< K, V > &mm)
Definition: UStl.h:46
rtabmap::EpipolarGeometry::setRansacParam2
void setRansacParam2(double ransacParam2)
Definition: EpipolarGeometry.h:59
rtabmap::EpipolarGeometry::findPairsAll
static int findPairsAll(const std::multimap< int, T > &wordsA, const std::multimap< int, T > &wordsB, std::list< std::pair< int, std::pair< T, T > > > &pairs, bool ignoreNegativeIds=true)
Definition: EpipolarGeometry.h:193
rtabmap::EpipolarGeometry::findPairs
static int findPairs(const std::multimap< int, T > &wordsA, const std::multimap< int, T > &wordsB, std::list< std::pair< int, std::pair< T, T > > > &pairs, bool ignoreNegativeIds=true)
Definition: EpipolarGeometry.h:124
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
rtabmap::EpipolarGeometry::getRansacParam1
double getRansacParam1() const
Definition: EpipolarGeometry.h:54
RTABMAP_EXP
#define RTABMAP_EXP
Definition: RtabmapExp.h:38
UStl.h
Wrappers of STL for convenient functions.
rtabmap::EpipolarGeometry::findPairs
static int findPairs(const std::map< int, T > &wordsA, const std::map< int, T > &wordsB, std::list< std::pair< int, std::pair< T, T > > > &pairs, bool ignoreNegativeIds=true)
Definition: EpipolarGeometry.h:96
rtabmap::EpipolarGeometry::setMatchCountMinAccepted
void setMatchCountMinAccepted(int matchCountMinAccepted)
Definition: EpipolarGeometry.h:57
rtabmap::EpipolarGeometry::getRansacParam2
double getRansacParam2() const
Definition: EpipolarGeometry.h:55
rtabmap::EpipolarGeometry::getMatchCountMinAccepted
int getMatchCountMinAccepted() const
Definition: EpipolarGeometry.h:53
uValues
std::vector< V > uValues(const std::multimap< K, V > &mm)
Definition: UStl.h:100
rtabmap::EpipolarGeometry::findPairsUnique
static int findPairsUnique(const std::multimap< int, T > &wordsA, const std::multimap< int, T > &wordsB, std::list< std::pair< int, std::pair< T, T > > > &pairs, bool ignoreNegativeIds=true)
Definition: EpipolarGeometry.h:158
rtabmap::EpipolarGeometry::setRansacParam1
void setRansacParam1(double ransacParam1)
Definition: EpipolarGeometry.h:58

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:58