30 #include "rtabmap/core/rtabmap_core_export.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>
64 static void findEpipolesFromF(
65 const cv::Mat & fundamentalMatrix,
69 static cv::Mat findPFromE(
76 static cv::Mat findFFromWords(
77 const std::list<std::pair<
int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
78 std::vector<uchar> & status,
79 double ransacReprojThreshold = 3.0,
80 double ransacConfidence = 0.99);
83 static void findRTFromP(
88 static cv::Mat findFFromCalibratedStereoCameras(
double fx,
double fy,
double cx,
double cy,
double Tx,
double Ty);
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)
102 int realPairsCount = 0;
104 for(
typename std::map<int, T>::const_iterator
i=wordsA.begin();
i!=wordsA.end(); ++
i)
106 if(!ignoreNegativeIds || (ignoreNegativeIds &&
i->first>=0))
108 std::map<int, cv::KeyPoint>::const_iterator ptB = wordsB.find(
i->first);
109 if(ptB != wordsB.end())
111 pairs.push_back(std::pair<
int, std::pair<T, T> >(
i->first, std::make_pair(
i->second, ptB->second)));
116 return realPairsCount;
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)
131 typename std::multimap<int, T>::const_iterator iterA;
132 typename std::multimap<int, T>::const_iterator iterB;
134 int realPairsCount = 0;
135 for(std::list<int>::const_iterator
i=ids.begin();
i!=ids.end(); ++
i)
137 if(!ignoreNegativeIds || (ignoreNegativeIds && *
i >= 0))
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)
143 pairs.push_back(std::pair<
int, std::pair<T, T> >(*
i, std::make_pair((*iterA).second, (*iterB).second)));
150 return realPairsCount;
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)
165 int realPairsCount = 0;
167 for(std::list<int>::const_iterator
i=ids.begin();
i!=ids.end(); ++
i)
169 if(!ignoreNegativeIds || (ignoreNegativeIds && *
i>=0))
171 std::list<T> ptsA =
uValues(wordsA, *
i);
172 std::list<T> ptsB =
uValues(wordsB, *
i);
173 if(ptsA.size() == 1 && ptsB.size() == 1)
175 pairs.push_back(std::pair<
int, std::pair<T, T> >(*
i, std::pair<T, T>(ptsA.front(), ptsB.front())));
178 else if(ptsA.size()>1 && ptsB.size()>1)
181 realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
185 return realPairsCount;
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)
201 int realPairsCount = 0;;
202 for(std::list<int>::const_iterator
iter=ids.begin();
iter!=ids.end(); ++
iter)
204 if(!ignoreNegativeIds || (ignoreNegativeIds && *
iter>=0))
209 realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
211 for(
typename std::list<T>::iterator jter=ptsA.begin(); jter!=ptsA.end(); ++jter)
213 for(
typename std::list<T>::iterator kter=ptsB.begin(); kter!=ptsB.end(); ++kter)
215 pairs.push_back(std::pair<
int, std::pair<T, T> >(*
iter, std::pair<T, T>(*jter, *kter)));
220 return realPairsCount;
223 static cv::Mat linearLSTriangulation(
229 static cv::Mat iterativeLinearLSTriangulation(
231 const cv::Matx34d &
P,
233 const cv::Matx34d & P1);
235 static double triangulatePoints(
236 const cv::Mat& pt_set1,
237 const cv::Mat& pt_set2,
240 pcl::PointCloud<pcl::PointXYZ>::Ptr & pointcloud,
241 std::vector<double> & reproj_errors);