solvepnp.cpp
Go to the documentation of this file.
1 /*M///////////////////////////////////////////////////////////////////////////////////////
2  //
3  // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
4  //
5  // By downloading, copying, installing or using the software you agree to this license.
6  // If you do not agree to this license, do not download, install,
7  // copy or use the software.
8  //
9  //
10  // License Agreement
11  // For Open Source Computer Vision Library
12  //
13  // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
14  // Copyright (C) 2009, Willow Garage Inc., all rights reserved.
15  // Third party copyrights are property of their respective owners.
16  //
17  // Redistribution and use in source and binary forms, with or without modification,
18  // are permitted provided that the following conditions are met:
19  //
20  // * Redistribution's of source code must retain the above copyright notice,
21  // this list of conditions and the following disclaimer.
22  //
23  // * Redistribution's in binary form must reproduce the above copyright notice,
24  // this list of conditions and the following disclaimer in the documentation
25  // and/or other materials provided with the distribution.
26  //
27  // * The name of the copyright holders may not be used to endorse or promote products
28  // derived from this software without specific prior written permission.
29  //
30  // This software is provided by the copyright holders and contributors "as is" and
31  // any express or implied warranties, including, but not limited to, the implied
32  // warranties of merchantability and fitness for a particular purpose are disclaimed.
33  // In no event shall the Intel Corporation or contributors be liable for any direct,
34  // indirect, incidental, special, exemplary, or consequential damages
35  // (including, but not limited to, procurement of substitute goods or services;
36  // loss of use, data, or profits; or business interruption) however caused
37  // and on any theory of liability, whether in contract, strict liability,
38  // or tort (including negligence or otherwise) arising in any way out of
39  // the use of this software, even if advised of the possibility of such damage.
40  //
41  //M*/
42 
43 #include "solvepnp.h"
44 #include <iostream>
45 
46 using namespace cv;
47 
48 namespace cv3
49 {
50 
52 {
53 
54 public:
55 
56  PnPRansacCallback(Mat _cameraMatrix=Mat(3,3,CV_64F), Mat _distCoeffs=Mat(4,1,CV_64F), int _flags=CV_ITERATIVE,
57  bool _useExtrinsicGuess=false, Mat _rvec=Mat(), Mat _tvec=Mat() )
58  : cameraMatrix(_cameraMatrix), distCoeffs(_distCoeffs), flags(_flags), useExtrinsicGuess(_useExtrinsicGuess),
59  rvec(_rvec), tvec(_tvec) {}
60 
61  /* Pre: True */
62  /* Post: compute _model with given points an return number of found models */
63  int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const
64  {
65  Mat opoints = _m1.getMat(), ipoints = _m2.getMat();
66 
67  bool correspondence = solvePnP( _m1, _m2, cameraMatrix, distCoeffs,
68  rvec, tvec, useExtrinsicGuess, flags );
69 
70  Mat _local_model;
71  hconcat(rvec, tvec, _local_model);
72  _local_model.copyTo(_model);
73 
74  return correspondence;
75  }
76 
77  /* Pre: True */
78  /* Post: fill _err with projection errors */
79  void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const
80  {
81 
82  Mat opoints = _m1.getMat(), ipoints = _m2.getMat(), model = _model.getMat();
83 
84  int i, count = opoints.checkVector(3);
85  Mat _rvec = model.col(0);
86  Mat _tvec = model.col(1);
87 
88 
89  Mat projpoints(count, 2, CV_32FC1);
90  projectPoints(opoints, _rvec, _tvec, cameraMatrix, distCoeffs, projpoints);
91 
92  const Point2f* ipoints_ptr = ipoints.ptr<Point2f>();
93  const Point2f* projpoints_ptr = projpoints.ptr<Point2f>();
94 
95  _err.create(count, 1, CV_32FC1);
96  float* err = _err.getMat().ptr<float>();
97 
98  for ( i = 0; i < count; ++i)
99  err[i] = (float)norm( ipoints_ptr[i] - projpoints_ptr[i] );
100 
101  }
102 
103 
106  int flags;
108  Mat rvec;
109  Mat tvec;
110 };
111 
112 bool solvePnPRansac(InputArray _opoints, InputArray _ipoints,
113  InputArray _cameraMatrix, InputArray _distCoeffs,
114  OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess,
115  int iterationsCount, float reprojectionError, double confidence,
116  OutputArray _inliers, int flags)
117 {
118 
119  Mat opoints0 = _opoints.getMat(), ipoints0 = _ipoints.getMat();
120  Mat opoints, ipoints;
121  if( opoints0.depth() == CV_64F || !opoints0.isContinuous() )
122  opoints0.convertTo(opoints, CV_32F);
123  else
124  opoints = opoints0;
125  if( ipoints0.depth() == CV_64F || !ipoints0.isContinuous() )
126  ipoints0.convertTo(ipoints, CV_32F);
127  else
128  ipoints = ipoints0;
129 
130  int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
131  CV_Assert( npoints >= 0 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );
132 
133  CV_Assert(opoints.isContinuous());
134  CV_Assert(opoints.depth() == CV_32F || opoints.depth() == CV_64F);
135  CV_Assert((opoints.rows == 1 && opoints.channels() == 3) || opoints.cols*opoints.channels() == 3);
136  CV_Assert(ipoints.isContinuous());
137  CV_Assert(ipoints.depth() == CV_32F || ipoints.depth() == CV_64F);
138  CV_Assert((ipoints.rows == 1 && ipoints.channels() == 2) || ipoints.cols*ipoints.channels() == 2);
139 
140  Mat rvec = useExtrinsicGuess ? _rvec.getMat() : Mat(3, 1, CV_64FC1);
141  Mat tvec = useExtrinsicGuess ? _tvec.getMat() : Mat(3, 1, CV_64FC1);
142  Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
143 
144  int model_points = 6;
145  int ransac_kernel_method = CV_EPNP;
146 
147  if( npoints == 4 )
148  {
149  model_points = 4;
150  ransac_kernel_method = CV_P3P;
151  }
152 
153  Ptr<PointSetRegistrator::Callback> cb; // pointer to callback
154  cb = Ptr<PnPRansacCallback>(new PnPRansacCallback( cameraMatrix, distCoeffs, ransac_kernel_method, useExtrinsicGuess, rvec, tvec));
155 
156  double param1 = reprojectionError; // reprojection error
157  double param2 = confidence; // confidence
158  int param3 = iterationsCount; // number maximum iterations
159 
160  Mat _local_model(3, 2, CV_64FC1);
161  Mat _mask_local_inliers(1, opoints.rows, CV_8UC1);
162 
163  // call Ransac
164  int result = createRANSACPointSetRegistrator(cb, model_points,
165  param1, param2, param3)->run(opoints, ipoints, _local_model, _mask_local_inliers);
166 
167  if( result > 0 )
168  {
169  std::vector<Point3d> opoints_inliers;
170  std::vector<Point2d> ipoints_inliers;
171  opoints.convertTo(opoints_inliers, CV_64F);
172  ipoints.convertTo(ipoints_inliers, CV_64F);
173 
174  const uchar* mask = _mask_local_inliers.ptr<uchar>();
175  int npoints1 = compressElems(&opoints_inliers[0], mask, 1, npoints);
176  compressElems(&ipoints_inliers[0], mask, 1, npoints);
177 
178  opoints_inliers.resize(npoints1);
179  ipoints_inliers.resize(npoints1);
180  result = solvePnP(opoints_inliers, ipoints_inliers, cameraMatrix,
181  distCoeffs, rvec, tvec, useExtrinsicGuess, flags == CV_P3P ? CV_EPNP : flags) ? 1 : -1;
182  }
183 
184  if( result <= 0 || _local_model.rows <= 0)
185  {
186  rvec.copyTo(_rvec); // output rotation vector
187  tvec.copyTo(_tvec); // output translation vector
188 
189  if( _inliers.needed() )
190  _inliers.release();
191 
192  return false;
193  }
194  else
195  {
196  _local_model.col(0).copyTo(_rvec); // output rotation vector
197  _local_model.col(1).copyTo(_tvec); // output translation vector
198  }
199 
200  if(_inliers.needed())
201  {
202  Mat _local_inliers;
203  for (int i = 0; i < npoints; ++i)
204  {
205  if((int)_mask_local_inliers.at<uchar>(i) != 0) // inliers mask
206  _local_inliers.push_back(i); // output inliers vector
207  }
208  _local_inliers.copyTo(_inliers);
209  }
210  return true;
211 }
212 
213 int RANSACUpdateNumIters( double p, double ep, int modelPoints, int maxIters )
214 {
215  if( modelPoints <= 0 )
216  CV_Error( 0, "the number of model points should be positive" );
217 
218  p = MAX(p, 0.);
219  p = MIN(p, 1.);
220  ep = MAX(ep, 0.);
221  ep = MIN(ep, 1.);
222 
223  // avoid inf's & nan's
224  double num = MAX(1. - p, DBL_MIN);
225  double denom = 1. - std::pow(1. - ep, modelPoints);
226  if( denom < DBL_MIN )
227  return 0;
228 
229  num = std::log(num);
230  denom = std::log(denom);
231 
232  return denom >= 0 || -num >= maxIters*(-denom) ? maxIters : cvRound(num/denom);
233 }
234 
236 {
237 public:
238  RANSACPointSetRegistrator(const Ptr<PointSetRegistrator::Callback>& _cb=Ptr<PointSetRegistrator::Callback>(),
239  int _modelPoints=0, double _threshold=0, double _confidence=0.99, int _maxIters=1000)
240  : cb(_cb), modelPoints(_modelPoints), threshold(_threshold), confidence(_confidence), maxIters(_maxIters)
241  {
242  checkPartialSubsets = false;
243  }
244 
245  int findInliers( const Mat& m1, const Mat& m2, const Mat& model, Mat& err, Mat& mask, double thresh ) const
246  {
247  cb->computeError( m1, m2, model, err );
248  mask.create(err.size(), CV_8U);
249 
250  CV_Assert( err.isContinuous() && err.type() == CV_32F && mask.isContinuous() && mask.type() == CV_8U);
251  const float* errptr = err.ptr<float>();
252  uchar* maskptr = mask.ptr<uchar>();
253  float t = (float)(thresh*thresh);
254  int i, n = (int)err.total(), nz = 0;
255  for( i = 0; i < n; i++ )
256  {
257  int f = errptr[i] <= t;
258  maskptr[i] = (uchar)f;
259  nz += f;
260  }
261  return nz;
262  }
263 
264  bool getSubset( const Mat& m1, const Mat& m2,
265  Mat& ms1, Mat& ms2, RNG& rng,
266  int maxAttempts=1000 ) const
267  {
268  cv::AutoBuffer<int> _idx(modelPoints);
269  int* idx = _idx;
270  int i = 0, j, k, iters = 0;
271  int esz1 = (int)m1.elemSize(), esz2 = (int)m2.elemSize();
272  int d1 = m1.channels() > 1 ? m1.channels() : m1.cols;
273  int d2 = m2.channels() > 1 ? m2.channels() : m2.cols;
274  int count = m1.checkVector(d1), count2 = m2.checkVector(d2);
275  const int *m1ptr = m1.ptr<int>(), *m2ptr = m2.ptr<int>();
276 
277  ms1.create(modelPoints, 1, CV_MAKETYPE(m1.depth(), d1));
278  ms2.create(modelPoints, 1, CV_MAKETYPE(m2.depth(), d2));
279 
280  int *ms1ptr = ms1.ptr<int>(), *ms2ptr = ms2.ptr<int>();
281 
282  CV_Assert( count >= modelPoints && count == count2 );
283  CV_Assert( (esz1 % sizeof(int)) == 0 && (esz2 % sizeof(int)) == 0 );
284  esz1 /= sizeof(int);
285  esz2 /= sizeof(int);
286 
287  for(; iters < maxAttempts; iters++)
288  {
289  for( i = 0; i < modelPoints && iters < maxAttempts; )
290  {
291  int idx_i = 0;
292  for(;;)
293  {
294  idx_i = idx[i] = rng.uniform(0, count);
295  for( j = 0; j < i; j++ )
296  if( idx_i == idx[j] )
297  break;
298  if( j == i )
299  break;
300  }
301  for( k = 0; k < esz1; k++ )
302  ms1ptr[i*esz1 + k] = m1ptr[idx_i*esz1 + k];
303  for( k = 0; k < esz2; k++ )
304  ms2ptr[i*esz2 + k] = m2ptr[idx_i*esz2 + k];
305  if( checkPartialSubsets && !cb->checkSubset( ms1, ms2, i+1 ))
306  {
307  // we may have selected some bad points;
308  // so, let's remove some of them randomly
309  i = rng.uniform(0, i+1);
310  iters++;
311  continue;
312  }
313  i++;
314  }
315  if( !checkPartialSubsets && i == modelPoints && !cb->checkSubset(ms1, ms2, i))
316  continue;
317  break;
318  }
319 
320  return i == modelPoints && iters < maxAttempts;
321  }
322 
323  bool run(InputArray _m1, InputArray _m2, OutputArray _model, OutputArray _mask) const
324  {
325  bool result = false;
326  Mat m1 = _m1.getMat(), m2 = _m2.getMat();
327  Mat err, mask, model, bestModel, ms1, ms2;
328 
329  int iter, niters = MAX(maxIters, 1);
330  int d1 = m1.channels() > 1 ? m1.channels() : m1.cols;
331  int d2 = m2.channels() > 1 ? m2.channels() : m2.cols;
332  int count = m1.checkVector(d1), count2 = m2.checkVector(d2), maxGoodCount = 0;
333 
334  RNG rng((uint64)-1);
335 
336  CV_Assert( cb );
337  CV_Assert( confidence > 0 && confidence < 1 );
338 
339  CV_Assert( count >= 0 && count2 == count );
340  if( count < modelPoints )
341  return false;
342 
343  Mat bestMask0, bestMask;
344 
345  if( _mask.needed() )
346  {
347  _mask.create(count, 1, CV_8U, -1, true);
348  bestMask0 = bestMask = _mask.getMat();
349  CV_Assert( (bestMask.cols == 1 || bestMask.rows == 1) && (int)bestMask.total() == count );
350  }
351  else
352  {
353  bestMask.create(count, 1, CV_8U);
354  bestMask0 = bestMask;
355  }
356 
357  if( count == modelPoints )
358  {
359  if( cb->runKernel(m1, m2, bestModel) <= 0 )
360  return false;
361  bestModel.copyTo(_model);
362  bestMask.setTo(Scalar::all(1));
363  return true;
364  }
365 
366  for( iter = 0; iter < niters; iter++ )
367  {
368  int i, goodCount, nmodels;
369  if( count > modelPoints )
370  {
371  bool found = getSubset( m1, m2, ms1, ms2, rng, 10000 );
372  if( !found )
373  {
374  if( iter == 0 )
375  return false;
376  break;
377  }
378  }
379 
380  nmodels = cb->runKernel( ms1, ms2, model );
381  if( nmodels <= 0 )
382  continue;
383  CV_Assert( model.rows % nmodels == 0 );
384  Size modelSize(model.cols, model.rows/nmodels);
385 
386  for( i = 0; i < nmodels; i++ )
387  {
388  Mat model_i = model.rowRange( i*modelSize.height, (i+1)*modelSize.height );
389  goodCount = findInliers( m1, m2, model_i, err, mask, threshold );
390 
391  if( goodCount > MAX(maxGoodCount, modelPoints-1) )
392  {
393  std::swap(mask, bestMask);
394  model_i.copyTo(bestModel);
395  maxGoodCount = goodCount;
396  niters = RANSACUpdateNumIters( confidence, (double)(count - goodCount)/count, modelPoints, niters );
397  }
398  }
399  }
400 
401  if( maxGoodCount > 0 )
402  {
403  if( bestMask.data != bestMask0.data )
404  {
405  if( bestMask.size() == bestMask0.size() )
406  bestMask.copyTo(bestMask0);
407  else
408  transpose(bestMask, bestMask0);
409  }
410  bestModel.copyTo(_model);
411  result = true;
412  }
413  else
414  _model.release();
415 
416  return result;
417  }
418 
419  void setCallback(const Ptr<PointSetRegistrator::Callback>& _cb) { cb = _cb; }
420 
421  Ptr<PointSetRegistrator::Callback> cb;
424  double threshold;
425  double confidence;
426  int maxIters;
427 };
428 
430 {
431 public:
432  LMeDSPointSetRegistrator(const Ptr<PointSetRegistrator::Callback>& _cb=Ptr<PointSetRegistrator::Callback>(),
433  int _modelPoints=0, double _confidence=0.99, int _maxIters=1000)
434  : RANSACPointSetRegistrator(_cb, _modelPoints, 0, _confidence, _maxIters) {}
435 
436  bool run(InputArray _m1, InputArray _m2, OutputArray _model, OutputArray _mask) const
437  {
438  const double outlierRatio = 0.45;
439  bool result = false;
440  Mat m1 = _m1.getMat(), m2 = _m2.getMat();
441  Mat ms1, ms2, err, errf, model, bestModel, mask, mask0;
442 
443  int d1 = m1.channels() > 1 ? m1.channels() : m1.cols;
444  int d2 = m2.channels() > 1 ? m2.channels() : m2.cols;
445  int count = m1.checkVector(d1), count2 = m2.checkVector(d2);
446  double minMedian = DBL_MAX, sigma;
447 
448  RNG rng((uint64)-1);
449 
450  CV_Assert( cb );
451  CV_Assert( confidence > 0 && confidence < 1 );
452 
453  CV_Assert( count >= 0 && count2 == count );
454  if( count < modelPoints )
455  return false;
456 
457  if( _mask.needed() )
458  {
459  _mask.create(count, 1, CV_8U, -1, true);
460  mask0 = mask = _mask.getMat();
461  CV_Assert( (mask.cols == 1 || mask.rows == 1) && (int)mask.total() == count );
462  }
463 
464  if( count == modelPoints )
465  {
466  if( cb->runKernel(m1, m2, bestModel) <= 0 )
467  return false;
468  bestModel.copyTo(_model);
469  mask.setTo(Scalar::all(1));
470  return true;
471  }
472 
473  int iter, niters = RANSACUpdateNumIters(confidence, outlierRatio, modelPoints, maxIters);
474  niters = MAX(niters, 3);
475 
476  for( iter = 0; iter < niters; iter++ )
477  {
478  int i, nmodels;
479  if( count > modelPoints )
480  {
481  bool found = getSubset( m1, m2, ms1, ms2, rng );
482  if( !found )
483  {
484  if( iter == 0 )
485  return false;
486  break;
487  }
488  }
489 
490  nmodels = cb->runKernel( ms1, ms2, model );
491  if( nmodels <= 0 )
492  continue;
493 
494  CV_Assert( model.rows % nmodels == 0 );
495  Size modelSize(model.cols, model.rows/nmodels);
496 
497  for( i = 0; i < nmodels; i++ )
498  {
499  Mat model_i = model.rowRange( i*modelSize.height, (i+1)*modelSize.height );
500  cb->computeError( m1, m2, model_i, err );
501  if( err.depth() != CV_32F )
502  err.convertTo(errf, CV_32F);
503  else
504  errf = err;
505  CV_Assert( errf.isContinuous() && errf.type() == CV_32F && (int)errf.total() == count );
506  std::sort(errf.ptr<int>(), errf.ptr<int>() + count);
507 
508  double median = count % 2 != 0 ?
509  errf.at<float>(count/2) : (errf.at<float>(count/2-1) + errf.at<float>(count/2))*0.5;
510 
511  if( median < minMedian )
512  {
513  minMedian = median;
514  model_i.copyTo(bestModel);
515  }
516  }
517  }
518 
519  if( minMedian < DBL_MAX )
520  {
521  sigma = 2.5*1.4826*(1 + 5./(count - modelPoints))*std::sqrt(minMedian);
522  sigma = MAX( sigma, 0.001 );
523 
524  count = findInliers( m1, m2, bestModel, err, mask, sigma );
525  if( _mask.needed() && mask0.data != mask.data )
526  {
527  if( mask0.size() == mask.size() )
528  mask.copyTo(mask0);
529  else
530  transpose(mask, mask0);
531  }
532  bestModel.copyTo(_model);
533  result = count >= modelPoints;
534  }
535  else
536  _model.release();
537 
538  return result;
539  }
540 
541 };
542 
543 Ptr<PointSetRegistrator> createRANSACPointSetRegistrator(const Ptr<PointSetRegistrator::Callback>& _cb,
544  int _modelPoints, double _threshold,
545  double _confidence, int _maxIters)
546 {
547  return Ptr<PointSetRegistrator>(
548  new RANSACPointSetRegistrator(_cb, _modelPoints, _threshold, _confidence, _maxIters));
549 }
550 
551 
552 Ptr<PointSetRegistrator> createLMeDSPointSetRegistrator(const Ptr<PointSetRegistrator::Callback>& _cb,
553  int _modelPoints, double _confidence, int _maxIters)
554 {
555  return Ptr<PointSetRegistrator>(
556  new LMeDSPointSetRegistrator(_cb, _modelPoints, _confidence, _maxIters));
557 }
558 
559 
561 {
562 public:
563  int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const
564  {
565  Mat m1 = _m1.getMat(), m2 = _m2.getMat();
566  const Point3f* from = m1.ptr<Point3f>();
567  const Point3f* to = m2.ptr<Point3f>();
568 
569  const int N = 12;
570  double buf[N*N + N + N];
571  Mat A(N, N, CV_64F, &buf[0]);
572  Mat B(N, 1, CV_64F, &buf[0] + N*N);
573  Mat X(N, 1, CV_64F, &buf[0] + N*N + N);
574  double* Adata = A.ptr<double>();
575  double* Bdata = B.ptr<double>();
576  A = Scalar::all(0);
577 
578  for( int i = 0; i < (N/3); i++ )
579  {
580  Bdata[i*3] = to[i].x;
581  Bdata[i*3+1] = to[i].y;
582  Bdata[i*3+2] = to[i].z;
583 
584  double *aptr = Adata + i*3*N;
585  for(int k = 0; k < 3; ++k)
586  {
587  aptr[0] = from[i].x;
588  aptr[1] = from[i].y;
589  aptr[2] = from[i].z;
590  aptr[3] = 1.0;
591  aptr += 16;
592  }
593  }
594 
595  solve(A, B, X, DECOMP_SVD);
596  X.reshape(1, 3).copyTo(_model);
597 
598  return 1;
599  }
600 
601  void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const
602  {
603  Mat m1 = _m1.getMat(), m2 = _m2.getMat(), model = _model.getMat();
604  const Point3f* from = m1.ptr<Point3f>();
605  const Point3f* to = m2.ptr<Point3f>();
606  const double* F = model.ptr<double>();
607 
608  int count = m1.checkVector(3);
609  CV_Assert( count > 0 );
610 
611  _err.create(count, 1, CV_32F);
612  Mat err = _err.getMat();
613  float* errptr = err.ptr<float>();
614 
615  for(int i = 0; i < count; i++ )
616  {
617  const Point3f& f = from[i];
618  const Point3f& t = to[i];
619 
620  double a = F[0]*f.x + F[1]*f.y + F[ 2]*f.z + F[ 3] - t.x;
621  double b = F[4]*f.x + F[5]*f.y + F[ 6]*f.z + F[ 7] - t.y;
622  double c = F[8]*f.x + F[9]*f.y + F[10]*f.z + F[11] - t.z;
623 
624  errptr[i] = (float)std::sqrt(a*a + b*b + c*c);
625  }
626  }
627 
628  bool checkSubset( InputArray _ms1, InputArray _ms2, int count ) const
629  {
630  const float threshold = 0.996f;
631  Mat ms1 = _ms1.getMat(), ms2 = _ms2.getMat();
632 
633  for( int inp = 1; inp <= 2; inp++ )
634  {
635  int j, k, i = count - 1;
636  const Mat* msi = inp == 1 ? &ms1 : &ms2;
637  const Point3f* ptr = msi->ptr<Point3f>();
638 
639  CV_Assert( count <= msi->rows );
640 
641  // check that the i-th selected point does not belong
642  // to a line connecting some previously selected points
643  for(j = 0; j < i; ++j)
644  {
645  Point3f d1 = ptr[j] - ptr[i];
646  float n1 = d1.x*d1.x + d1.y*d1.y;
647 
648  for(k = 0; k < j; ++k)
649  {
650  Point3f d2 = ptr[k] - ptr[i];
651  float denom = (d2.x*d2.x + d2.y*d2.y)*n1;
652  float num = d1.x*d2.x + d1.y*d2.y;
653 
654  if( num*num > threshold*threshold*denom )
655  return false;
656  }
657  }
658  }
659  return true;
660  }
661 };
662 
663 }
GLM_FUNC_DECL genType log(genType const &x)
bool getSubset(const Mat &m1, const Mat &m2, Mat &ms1, Mat &ms2, RNG &rng, int maxAttempts=1000) const
Definition: solvepnp.cpp:264
GLM_FUNC_DECL genIType mask(genIType const &count)
int RANSACUpdateNumIters(double p, double ep, int modelPoints, int maxIters)
Definition: solvepnp.cpp:213
Ptr< PointSetRegistrator > createRANSACPointSetRegistrator(const Ptr< PointSetRegistrator::Callback > &_cb, int _modelPoints, double _threshold, double _confidence, int _maxIters)
Definition: solvepnp.cpp:543
int runKernel(InputArray _m1, InputArray _m2, OutputArray _model) const
Definition: solvepnp.cpp:63
Ptr< PointSetRegistrator::Callback > cb
Definition: solvepnp.cpp:421
f
#define MAX(A, B)
Definition: sqlite3.c:8236
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
unsigned char uchar
Definition: matrix.h:41
RANSACPointSetRegistrator(const Ptr< PointSetRegistrator::Callback > &_cb=Ptr< PointSetRegistrator::Callback >(), int _modelPoints=0, double _threshold=0, double _confidence=0.99, int _maxIters=1000)
Definition: solvepnp.cpp:238
GLM_FUNC_DECL bool all(vecType< bool, P > const &v)
void swap(linb::any &lhs, linb::any &rhs) noexcept
void computeError(InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err) const
Definition: solvepnp.cpp:79
bool run(InputArray _m1, InputArray _m2, OutputArray _model, OutputArray _mask) const
Definition: solvepnp.cpp:323
Definition: Features2d.h:41
bool solvePnPRansac(InputArray _opoints, InputArray _ipoints, InputArray _cameraMatrix, InputArray _distCoeffs, OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, int iterationsCount, float reprojectionError, double confidence, OutputArray _inliers, int flags)
Definition: solvepnp.cpp:112
#define MIN(A, B)
Definition: sqlite3.c:8235
bool run(InputArray _m1, InputArray _m2, OutputArray _model, OutputArray _mask) const
Definition: solvepnp.cpp:436
void computeError(InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err) const
Definition: solvepnp.cpp:601
bool checkSubset(InputArray _ms1, InputArray _ms2, int count) const
Definition: solvepnp.cpp:628
int compressElems(T *ptr, const uchar *mask, int mstep, int count)
Definition: solvepnp.h:141
unsigned long long uint64
Definition: type_int.hpp:65
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
int runKernel(InputArray _m1, InputArray _m2, OutputArray _model) const
Definition: solvepnp.cpp:563
int findInliers(const Mat &m1, const Mat &m2, const Mat &model, Mat &err, Mat &mask, double thresh) const
Definition: solvepnp.cpp:245
model
Definition: trace.py:4
LMeDSPointSetRegistrator(const Ptr< PointSetRegistrator::Callback > &_cb=Ptr< PointSetRegistrator::Callback >(), int _modelPoints=0, double _confidence=0.99, int _maxIters=1000)
Definition: solvepnp.cpp:432
const int d1
Definition: SuperPoint.cc:20
PnPRansacCallback(Mat _cameraMatrix=Mat(3, 3, CV_64F), Mat _distCoeffs=Mat(4, 1, CV_64F), int _flags=CV_ITERATIVE, bool _useExtrinsicGuess=false, Mat _rvec=Mat(), Mat _tvec=Mat())
Definition: solvepnp.cpp:56
void setCallback(const Ptr< PointSetRegistrator::Callback > &_cb)
Definition: solvepnp.cpp:419
Ptr< PointSetRegistrator > createLMeDSPointSetRegistrator(const Ptr< PointSetRegistrator::Callback > &_cb, int _modelPoints, double _confidence, int _maxIters)
Definition: solvepnp.cpp:552


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:30