EpipolarGeometry.cpp
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 #include "rtabmap/core/EpipolarGeometry.h"
29 #include "rtabmap/core/Signature.h"
30 #include "rtabmap/utilite/ULogger.h"
31 #include "rtabmap/utilite/UTimer.h"
32 #include "rtabmap/utilite/UStl.h"
33 #include "rtabmap/utilite/UMath.h"
34 
35 #include <opencv2/core/core.hpp>
36 #include <opencv2/core/core_c.h>
37 #include <opencv2/calib3d/calib3d.hpp>
38 #include <iostream>
39 
40 namespace rtabmap
41 {
42 
44 // HypVerificatorEpipolarGeo
46 EpipolarGeometry::EpipolarGeometry(const ParametersMap & parameters) :
47  _matchCountMinAccepted(Parameters::defaultVhEpMatchCountMin()),
48  _ransacParam1(Parameters::defaultVhEpRansacParam1()),
49  _ransacParam2(Parameters::defaultVhEpRansacParam2())
50 {
51  this->parseParameters(parameters);
52 }
53 
54 EpipolarGeometry::~EpipolarGeometry() {
55 
56 }
57 
58 void EpipolarGeometry::parseParameters(const ParametersMap & parameters)
59 {
60  Parameters::parse(parameters, Parameters::kVhEpMatchCountMin(), _matchCountMinAccepted);
61  Parameters::parse(parameters, Parameters::kVhEpRansacParam1(), _ransacParam1);
62  Parameters::parse(parameters, Parameters::kVhEpRansacParam2(), _ransacParam2);
63 }
64 
65 bool EpipolarGeometry::check(const Signature * ssA, const Signature * ssB)
66 {
67  if(ssA == 0 || ssB == 0)
68  {
69  return false;
70  }
71  ULOGGER_DEBUG("id(%d,%d)", ssA->id(), ssB->id());
72 
73  std::list<std::pair<int, std::pair<int, int> > > pairsId;
74 
75  findPairsUnique(ssA->getWords(), ssB->getWords(), pairsId);
76 
77  if((int)pairsId.size()<_matchCountMinAccepted)
78  {
79  return false;
80  }
81 
82  std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
83  for(std::list<std::pair<int, std::pair<int, int> > >::iterator iter = pairsId.begin(); iter!=pairsId.end(); ++iter)
84  {
85  pairs.push_back(std::make_pair(iter->first, std::make_pair(ssA->getWordsKpts()[iter->second.first], ssB->getWordsKpts()[iter->second.second])));
86  }
87 
88  std::vector<uchar> status;
89  cv::Mat f = findFFromWords(pairs, status, _ransacParam1, _ransacParam2);
90 
91  int inliers = uSum(status);
92  if(inliers < _matchCountMinAccepted)
93  {
94  ULOGGER_DEBUG("Epipolar constraint failed A : not enough inliers (%d/%d), min is %d", inliers, pairs.size(), _matchCountMinAccepted);
95  return false;
96  }
97  else
98  {
99  UDEBUG("inliers = %d/%d", inliers, pairs.size());
100  return true;
101  }
102 }
103 
104 //STATIC STUFF
105 //Epipolar geometry
106 void EpipolarGeometry::findEpipolesFromF(const cv::Mat & fundamentalMatrix, cv::Vec3d & e1, cv::Vec3d & e2)
107 {
108  if(fundamentalMatrix.rows != 3 || fundamentalMatrix.cols != 3)
109  {
110  ULOGGER_ERROR("The matrix is not the good size...");
111  return;
112  }
113 
114  if(fundamentalMatrix.type() != CV_64FC1)
115  {
116  ULOGGER_ERROR("The matrix is not the good type...");
117  return;
118  }
119 
120 
121  cv::SVD svd(fundamentalMatrix);
122  cv::Mat u = svd.u;
123  cv::Mat v = svd.vt;
124  cv::Mat w = svd.w;
125 
126  // v is for image 1
127  // u is for image 2
128 
129  e1[0] = v.at<double>(0,2);// /v->data.db[2*3+2];
130  e1[1] = v.at<double>(1,2);// /v->data.db[2*3+2];
131  e1[2] = v.at<double>(2,2);// /v->data.db[2*3+2];
132 
133  e2[0] = u.at<double>(0,2);// /u->data.db[2*3+2];
134  e2[1] = u.at<double>(1,2);// /u->data.db[2*3+2];
135  e2[2] = u.at<double>(2,2);// /u->data.db[2*3+2];
136 }
137 
138 int inFrontOfBothCameras(const cv::Mat & x, const cv::Mat & xp, const cv::Mat & R, const cv::Mat & T)
139 {
140  // P0 matrix 3X4
141  cv::Mat p0 = cv::Mat::zeros(3, 4, CV_64FC1);
142  p0.at<double>(0,0) = 1;
143  p0.at<double>(1,1) = 1;
144  p0.at<double>(2,2) = 1;
145  cv::Mat p = cv::Mat::zeros(3, 4, CV_64FC1);
146  p.at<double>(0,0) = R.at<double>(0,0);
147  p.at<double>(0,1) = R.at<double>(0,1);
148  p.at<double>(0,2) = R.at<double>(0,2);
149  p.at<double>(1,0) = R.at<double>(1,0);
150  p.at<double>(1,1) = R.at<double>(1,1);
151  p.at<double>(1,2) = R.at<double>(1,2);
152  p.at<double>(2,0) = R.at<double>(2,0);
153  p.at<double>(2,1) = R.at<double>(2,1);
154  p.at<double>(2,2) = R.at<double>(2,2);
155  p.at<double>(0,3) = T.at<double>(0,0);
156  p.at<double>(1,3) = T.at<double>(1,0);
157  p.at<double>(2,3) = T.at<double>(2,0);
158 
159  cv::Mat pts4D;
160  //std::vector<double> reprojErrors;
161  //pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
162  //EpipolarGeometry::triangulatePoints(x, xp, p0, p, cloud, reprojErrors);
163  cv::triangulatePoints(p0, p, x, xp, pts4D);
164 
165  //http://en.wikipedia.org/wiki/Essential_matrix#3D_points_from_corresponding_image_points
166  int nValid = 0;
167  for(int i=0; i<x.cols; ++i)
168  {
169  // the five to ignore when all points are super close to the camera
170  if(pts4D.at<double>(2,i)/pts4D.at<double>(3,i) > 5)
171  //if(cloud->at(i).z > 5)
172  {
173  ++nValid;
174  }
175  }
176  UDEBUG("nValid=%d/%d", nValid, x.cols);
177 
178  return nValid;
179 }
180 
181 //Assuming P0 = [eye(3) zeros(3,1)]
182 // x1 and x2 are 2D points
183 // return camera matrix P (3x4) matrix
184 //http://www.robots.ox.ac.uk/~vgg/hzbook/hzbook2/HZepipolar.pdf
185 cv::Mat EpipolarGeometry::findPFromE(const cv::Mat & E,
186  const cv::Mat & x,
187  const cv::Mat & xp)
188 {
189  UDEBUG("begin");
190  UASSERT(E.rows == 3 && E.cols == 3);
191  UASSERT(E.type() == CV_64FC1);
192  UASSERT(x.rows == 2 && x.cols>0 && x.type() == CV_64FC1);
193  UASSERT(xp.rows == 2 && xp.cols>0 && x.type() == CV_64FC1);
194 
195  // skew matrix 3X3
196  cv::Mat w = cv::Mat::zeros( 3, 3, CV_64FC1);
197  w.at<double>(0,1) = -1;
198  w.at<double>(1,0) = 1;
199  w.at<double>(2,2) = 1;
200  //std::cout << "W=" << w << std::endl;
201 
202  cv::Mat e = E;
203  cv::SVD svd(e,cv::SVD::MODIFY_A);
204  cv::Mat u = svd.u;
205  cv::Mat vt = svd.vt;
206  cv::Mat s = svd.w;
207 
208  //std::cout << "u=" << u << std::endl;
209  //std::cout << "vt=" << vt << std::endl;
210  //std::cout << "s=" << s << std::endl;
211 
212  // E = u*diag(1,1,0)*vt
213  cv::Mat diag = cv::Mat::eye(3,3,CV_64FC1);
214  diag.at<double>(2,2) = 0;
215  e = u*diag*vt;
216  svd(e,cv::SVD::MODIFY_A);
217  u = svd.u;
218  vt = svd.vt;
219  s = svd.w;
220 
221  cv::Mat r = u*w*vt;
222  if(cv::determinant(r)+1.0 < 1e-09) {
223  //according to http://en.wikipedia.org/wiki/Essential_matrix#Showing_that_it_is_valid
224  UDEBUG("det(R) == -1 [%f]: flip E's sign", cv::determinant(r));
225  e = -E;
226  svd(e,cv::SVD::MODIFY_A);
227  u = svd.u;
228  vt = svd.vt;
229  s = svd.w;
230  }
231  cv::Mat wt = w.t();
232 
233  // INFO: There 4 cases of P, only one have all the points in
234  // front of the two cameras (positive z).
235 
236  cv::Mat r1 = u*w*vt;
237  cv::Mat r2 = u*wt*vt;
238 
239  cv::Mat t1 = u.col(2);
240  cv::Mat t2 = u.col(2)*-1;
241 
242  int max = 0;
243  int maxIndex = 1;
244  int maxTmp;
245  cv::Mat R=r1,T=t1;
246 
247  // Case 1 : P = [U*W*V' t];
248  max = inFrontOfBothCameras(x, xp, r1, t1);
249  // Case 2 : P = [U*W*V' -t];
250  maxTmp = inFrontOfBothCameras(x, xp, r1, t2);
251  if(maxTmp > max)
252  {
253  maxIndex = 2;
254  max = maxTmp;
255  R=r1,T=t2;
256  }
257  // Case 3 : P = [U*W'*V' t];
258  maxTmp = inFrontOfBothCameras(x, xp, r2, t1);
259  if(maxTmp > max)
260  {
261  maxIndex = 3;
262  max = maxTmp;
263  R=r2,T=t1;
264  }
265  // Case 4 : P = [U*W'*V' -t];
266  maxTmp = inFrontOfBothCameras(x, xp, r2, t2);
267  if(maxTmp > max)
268  {
269  maxIndex = 4;
270  max = maxTmp;
271  R=r2,T=t2;
272  }
273 
274  if(max > 0)
275  {
276  UDEBUG("Case %d", maxIndex);
277 
278  // P matrix 3x4
279  cv::Mat p = cv::Mat::zeros(3, 4, CV_64FC1);
280  p.at<double>(0,0) = R.at<double>(0,0);
281  p.at<double>(0,1) = R.at<double>(0,1);
282  p.at<double>(0,2) = R.at<double>(0,2);
283  p.at<double>(1,0) = R.at<double>(1,0);
284  p.at<double>(1,1) = R.at<double>(1,1);
285  p.at<double>(1,2) = R.at<double>(1,2);
286  p.at<double>(2,0) = R.at<double>(2,0);
287  p.at<double>(2,1) = R.at<double>(2,1);
288  p.at<double>(2,2) = R.at<double>(2,2);
289  p.at<double>(0,3) = T.at<double>(0);
290  p.at<double>(1,3) = T.at<double>(1);
291  p.at<double>(2,3) = T.at<double>(2);
292  return p;
293  }
294 
295  return cv::Mat();
296 }
297 
298 cv::Mat EpipolarGeometry::findFFromWords(
299  const std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs, // id, kpt1, kpt2
300  std::vector<uchar> & status,
301  double ransacReprojThreshold,
302  double ransacConfidence)
303 {
304 
305  status = std::vector<uchar>(pairs.size(), 0);
306  //Convert Keypoints to a structure that OpenCV understands
307  //3 dimensions (Homogeneous vectors)
308  cv::Mat points1(1, pairs.size(), CV_32FC2);
309  cv::Mat points2(1, pairs.size(), CV_32FC2);
310 
311  float * points1data = points1.ptr<float>(0);
312  float * points2data = points2.ptr<float>(0);
313 
314  // Fill the points here ...
315  int i=0;
316  for(std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > >::const_iterator iter = pairs.begin();
317  iter != pairs.end();
318  ++iter )
319  {
320  points1data[i*2] = (*iter).second.first.pt.x;
321  points1data[i*2+1] = (*iter).second.first.pt.y;
322 
323  points2data[i*2] = (*iter).second.second.pt.x;
324  points2data[i*2+1] = (*iter).second.second.pt.y;
325 
326  ++i;
327  }
328 
329  UTimer timer;
330  timer.start();
331 
332  // Find the fundamental matrix
333  cv::Mat fundamentalMatrix = cv::findFundamentalMat(
334  points1,
335  points2,
336  status,
337  cv::FM_RANSAC,
338  ransacReprojThreshold,
339  ransacConfidence);
340 
341  ULOGGER_DEBUG("Find fundamental matrix (OpenCV) time = %fs", timer.ticks());
342 
343  // Fundamental matrix is valid ?
344  bool fundMatFound = false;
345  UASSERT(fundamentalMatrix.type() == CV_64FC1);
346  if(fundamentalMatrix.cols==3 && fundamentalMatrix.rows==3 &&
347  (fundamentalMatrix.at<double>(0,0) != 0.0 ||
348  fundamentalMatrix.at<double>(0,1) != 0.0 ||
349  fundamentalMatrix.at<double>(0,2) != 0.0 ||
350  fundamentalMatrix.at<double>(1,0) != 0.0 ||
351  fundamentalMatrix.at<double>(1,1) != 0.0 ||
352  fundamentalMatrix.at<double>(1,2) != 0.0 ||
353  fundamentalMatrix.at<double>(2,0) != 0.0 ||
354  fundamentalMatrix.at<double>(2,1) != 0.0 ||
355  fundamentalMatrix.at<double>(2,2) != 0.0) )
356 
357  {
358  fundMatFound = true;
359  }
360 
361  ULOGGER_DEBUG("fm_count=%d...", fundMatFound);
362 
363  if(fundMatFound)
364  {
365  // Show the fundamental matrix
366  UDEBUG(
367  "F = [%f %f %f;%f %f %f;%f %f %f]",
368  fundamentalMatrix.ptr<double>(0)[0],
369  fundamentalMatrix.ptr<double>(0)[1],
370  fundamentalMatrix.ptr<double>(0)[2],
371  fundamentalMatrix.ptr<double>(0)[3],
372  fundamentalMatrix.ptr<double>(0)[4],
373  fundamentalMatrix.ptr<double>(0)[5],
374  fundamentalMatrix.ptr<double>(0)[6],
375  fundamentalMatrix.ptr<double>(0)[7],
376  fundamentalMatrix.ptr<double>(0)[8]);
377  }
378  return fundamentalMatrix;
379 }
380 
381 void EpipolarGeometry::findRTFromP(
382  const cv::Mat & p,
383  cv::Mat & r,
384  cv::Mat & t)
385 {
386  UASSERT(p.cols == 4 && p.rows == 3);
387  r = cv::Mat(p, cv::Range(0,3), cv::Range(0,3));
388  //r = -r.inv();
389  //t = r*p.col(3);
390  t = p.col(3);
391 }
392 
393 cv::Mat EpipolarGeometry::findFFromCalibratedStereoCameras(double fx, double fy, double cx, double cy, double Tx, double Ty)
394 {
395  cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
396 
397  double Bx = Tx/-fx;
398  double By = Ty/-fy;
399 
400  cv::Mat tx = (cv::Mat_<double>(3,3) <<
401  0, 0, By,
402  0, 0, -Bx,
403  -By, Bx, 0);
404 
405  cv::Mat K = (cv::Mat_<double>(3,3) <<
406  fx, 0, cx,
407  0, fy, cy,
408  0, 0, 1);
409 
410  cv::Mat E = tx*R;
411 
412  return K.inv().t()*E*K.inv();
413 }
414 
415 
421 cv::Mat EpipolarGeometry::linearLSTriangulation(
422  cv::Point3d u, //homogenous image point (u,v,1)
423  cv::Matx34d P, //camera 1 matrix 3x4 double
424  cv::Point3d u1, //homogenous image point in 2nd camera
425  cv::Matx34d P1 //camera 2 matrix 3x4 double
426  )
427 {
428  //build matrix A for homogenous equation system Ax = 0
429  //assume X = (x,y,z,1), for Linear-LS method
430  //which turns it into a AX = B system, where A is 4x3, X is 3x1 and B is 4x1
431  cv::Mat A = (cv::Mat_<double>(4,3) <<
432  u.x*P(2,0)-P(0,0), u.x*P(2,1)-P(0,1), u.x*P(2,2)-P(0,2),
433  u.y*P(2,0)-P(1,0), u.y*P(2,1)-P(1,1), u.y*P(2,2)-P(1,2),
434  u1.x*P1(2,0)-P1(0,0), u1.x*P1(2,1)-P1(0,1), u1.x*P1(2,2)-P1(0,2),
435  u1.y*P1(2,0)-P1(1,0), u1.y*P1(2,1)-P1(1,1), u1.y*P1(2,2)-P1(1,2)
436  );
437  cv::Mat B = (cv::Mat_<double>(4,1) <<
438  -(u.x*P(2,3) -P(0,3)),
439  -(u.y*P(2,3) -P(1,3)),
440  -(u1.x*P1(2,3) -P1(0,3)),
441  -(u1.y*P1(2,3) -P1(1,3)));
442 
443  cv::Mat X;
444  solve(A,B,X,cv::DECOMP_SVD);
445 
446  return X; // return 1x3 double
447 }
448 
454 cv::Mat EpipolarGeometry::iterativeLinearLSTriangulation(
455  cv::Point3d u, //homogenous image point (u,v,1)
456  const cv::Matx34d & P, //camera 1 matrix 3x4 double
457  cv::Point3d u1, //homogenous image point in 2nd camera
458  const cv::Matx34d & P1) //camera 2 matrix 3x4 double
459 {
460  double wi = 1, wi1 = 1;
461  double EPSILON = 0.0001;
462 
463  cv::Mat X(4,1,CV_64FC1);
464  cv::Mat X_ = linearLSTriangulation(u,P,u1,P1);
465  X.at<double>(0) = X_.at<double>(0);
466  X.at<double>(1) = X_.at<double>(1);
467  X.at<double>(2) = X_.at<double>(2);
468  X.at<double>(3) = 1.0;
469  for (int i=0; i<10; i++) //Hartley suggests 10 iterations at most
470  {
471  //recalculate weights
472  double p2x = cv::Mat(cv::Mat(P).row(2)*X).at<double>(0);
473  double p2x1 = cv::Mat(cv::Mat(P1).row(2)*X).at<double>(0);
474 
475  //breaking point
476  if(fabs(wi - p2x) <= EPSILON && fabs(wi1 - p2x1) <= EPSILON) break;
477 
478  wi = p2x;
479  wi1 = p2x1;
480 
481  //reweight equations and solve
482  cv::Mat A = (cv::Mat_<double>(4,3) <<
483  (u.x*P(2,0)-P(0,0))/wi, (u.x*P(2,1)-P(0,1))/wi, (u.x*P(2,2)-P(0,2))/wi,
484  (u.y*P(2,0)-P(1,0))/wi, (u.y*P(2,1)-P(1,1))/wi, (u.y*P(2,2)-P(1,2))/wi,
485  (u1.x*P1(2,0)-P1(0,0))/wi1, (u1.x*P1(2,1)-P1(0,1))/wi1, (u1.x*P1(2,2)-P1(0,2))/wi1,
486  (u1.y*P1(2,0)-P1(1,0))/wi1, (u1.y*P1(2,1)-P1(1,1))/wi1, (u1.y*P1(2,2)-P1(1,2))/wi1);
487  cv::Mat B = (cv::Mat_<double>(4,1) <<
488  -(u.x*P(2,3) -P(0,3))/wi,
489  -(u.y*P(2,3) -P(1,3))/wi,
490  -(u1.x*P1(2,3) -P1(0,3))/wi1,
491  -(u1.y*P1(2,3) -P1(1,3))/wi1);
492 
493  solve(A,B,X_,cv::DECOMP_SVD);
494  X.at<double>(0) = X_.at<double>(0);
495  X.at<double>(1) = X_.at<double>(1);
496  X.at<double>(2) = X_.at<double>(2);
497  X.at<double>(3) = 1.0;
498  }
499  return X; // return 4x1 double
500 }
501 
505 //Triagulate points
506 double EpipolarGeometry::triangulatePoints(
507  const cv::Mat& pt_set, //2xN double
508  const cv::Mat& pt_set1, //2xN double
509  const cv::Mat& P, // 3x4 double
510  const cv::Mat& P1, // 3x4 double
511  pcl::PointCloud<pcl::PointXYZ>::Ptr & pointcloud,
512  std::vector<double> & reproj_errors)
513 {
514  pointcloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
515 
516  unsigned int pts_size = pt_set.cols;
517 
518  pointcloud->resize(pts_size);
519  reproj_errors.resize(pts_size);
520 
521  for(unsigned int i=0; i<pts_size; i++)
522  {
523  cv::Point3d u(pt_set.at<double>(0,i),pt_set.at<double>(1,i),1.0);
524  cv::Point3d u1(pt_set1.at<double>(0,i),pt_set1.at<double>(1,i),1.0);
525 
526  cv::Mat X = iterativeLinearLSTriangulation(u,P,u1,P1);
527 
528  cv::Mat x_proj = P * X; //reproject
529  x_proj = x_proj / x_proj.at<double>(2);
530  cv::Point3d xPt_img_(x_proj.at<double>(0), x_proj.at<double>(1), 1.0);
531 
532  double reprj_err = norm(xPt_img_ - u);
533  reproj_errors[i] = reprj_err;
534  pointcloud->at(i) = pcl::PointXYZ(X.at<double>(0),X.at<double>(1),X.at<double>(2));
535  }
536 
537  return cv::mean(reproj_errors)[0]; // mean reproj error
538 }
539 
540 } // namespace rtabmap
w
RowVector3d w
rtabmap::EpipolarGeometry::_ransacParam2
double _ransacParam2
Definition: EpipolarGeometry.h:246
cy
const double cy
timer
s
RealScalar s
uSum
T uSum(const std::list< T > &list)
Definition: UMath.h:318
fx
const double fx
rtabmap::Signature::getWords
const std::multimap< int, int > & getWords() const
Definition: Signature.h:112
diag
Matrix diag(const std::vector< Matrix > &Hs)
rtabmap::Signature::getWordsKpts
const std::vector< cv::KeyPoint > & getWordsKpts() const
Definition: Signature.h:113
B
rtabmap::EpipolarGeometry::parseParameters
void parseParameters(const ParametersMap &parameters)
Definition: EpipolarGeometry.cpp:58
E
int E
rtabmap::EpipolarGeometry::_ransacParam1
double _ransacParam1
Definition: EpipolarGeometry.h:245
iterator
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
svd
cout<< "Here is the matrix m:"<< endl<< m<< endl;JacobiSVD< MatrixXf > svd(m, ComputeThinU|ComputeThinV)
UTimer.h
ULOGGER_DEBUG
#define ULOGGER_DEBUG(...)
Definition: ULogger.h:53
EpipolarGeometry.h
rtabmap::EpipolarGeometry::findFFromCalibratedStereoCameras
static cv::Mat findFFromCalibratedStereoCameras(double fx, double fy, double cx, double cy, double Tx, double Ty)
Definition: EpipolarGeometry.cpp:393
UMath.h
Basic mathematics functions.
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:500
fabs
Real fabs(const Real &a)
rtabmap::EpipolarGeometry::EpipolarGeometry
EpipolarGeometry(const ParametersMap &parameters=ParametersMap())
Definition: EpipolarGeometry.cpp:46
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
X
const char X
A
rtabmap::EpipolarGeometry::check
bool check(const Signature *ssA, const Signature *ssB)
Definition: EpipolarGeometry.cpp:65
rtabmap::EpipolarGeometry::findPFromE
static cv::Mat findPFromE(const cv::Mat &E, const cv::Mat &x, const cv::Mat &xp)
Definition: EpipolarGeometry.cpp:185
rtabmap::EpipolarGeometry::findFFromWords
static cv::Mat findFFromWords(const std::list< std::pair< int, std::pair< cv::KeyPoint, cv::KeyPoint > > > &pairs, std::vector< uchar > &status, double ransacReprojThreshold=3.0, double ransacConfidence=0.99)
Definition: EpipolarGeometry.cpp:298
Signature.h
rtabmap::EpipolarGeometry::triangulatePoints
static double triangulatePoints(const cv::Mat &pt_set1, const cv::Mat &pt_set2, const cv::Mat &P, const cv::Mat &P1, pcl::PointCloud< pcl::PointXYZ >::Ptr &pointcloud, std::vector< double > &reproj_errors)
Definition: EpipolarGeometry.cpp:506
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
rtabmap::EpipolarGeometry::_matchCountMinAccepted
int _matchCountMinAccepted
Definition: EpipolarGeometry.h:244
glm::row
GLM_FUNC_DECL genType::row_type row(genType const &m, length_t const &index)
rtabmap::EpipolarGeometry::linearLSTriangulation
static cv::Mat linearLSTriangulation(cv::Point3d u, cv::Matx34d P, cv::Point3d u1, cv::Matx34d P1)
Definition: EpipolarGeometry.cpp:421
UASSERT
#define UASSERT(condition)
x
x
p
Point3_ p(2)
rtabmap::EpipolarGeometry::iterativeLinearLSTriangulation
static cv::Mat iterativeLinearLSTriangulation(cv::Point3d u, const cv::Matx34d &P, cv::Point3d u1, const cv::Matx34d &P1)
Definition: EpipolarGeometry.cpp:454
Eigen::Triplet< double >
rtabmap::EpipolarGeometry::~EpipolarGeometry
virtual ~EpipolarGeometry()
Definition: EpipolarGeometry.cpp:54
rtabmap::EpipolarGeometry::findEpipolesFromF
static void findEpipolesFromF(const cv::Mat &fundamentalMatrix, cv::Vec3d &e1, cv::Vec3d &e2)
Definition: EpipolarGeometry.cpp:106
rtabmap::Parameters
Definition: Parameters.h:170
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
K
K
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
ULogger.h
ULogger class and convenient macros.
rtabmap::Signature::id
int id() const
Definition: Signature.h:70
PointMatcherSupport::Parametrizable
iter
iterator iter(handle obj)
p0
Vector3f p0
ULOGGER_ERROR
#define ULOGGER_ERROR(...)
Definition: ULogger.h:56
rtabmap::EpipolarGeometry::findRTFromP
static void findRTFromP(const cv::Mat &p, cv::Mat &r, cv::Mat &t)
Definition: EpipolarGeometry.cpp:381
UStl.h
Wrappers of STL for convenient functions.
v
Array< int, Dynamic, 1 > v
UDEBUG
#define UDEBUG(...)
UTimer
Definition: UTimer.h:46
cx
const double cx
t
Point2 t(10, 10)
rtabmap
Definition: CameraARCore.cpp:35
i
int i
fy
const double fy
rtabmap::Signature
Definition: Signature.h:48
rtabmap::inFrontOfBothCameras
int inFrontOfBothCameras(const cv::Mat &x, const cv::Mat &xp, const cv::Mat &R, const cv::Mat &T)
Definition: EpipolarGeometry.cpp:138
R
R

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:09