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  _rvec.create(3, 1, CV_64FC1);
141  _tvec.create(3, 1, CV_64FC1);
142 
143  Mat rvec = useExtrinsicGuess ? _rvec.getMat() : Mat(3, 1, CV_64FC1);
144  Mat tvec = useExtrinsicGuess ? _tvec.getMat() : Mat(3, 1, CV_64FC1);
145  Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
146 
147  int model_points = 6;
148  int ransac_kernel_method = CV_EPNP;
149 
150  if( npoints == 4 )
151  {
152  model_points = 4;
153  ransac_kernel_method = CV_P3P;
154  }
155 
156  Ptr<PointSetRegistrator::Callback> cb; // pointer to callback
157  cb = Ptr<PnPRansacCallback>(new PnPRansacCallback( cameraMatrix, distCoeffs, ransac_kernel_method, useExtrinsicGuess, rvec, tvec));
158 
159  double param1 = reprojectionError; // reprojection error
160  double param2 = confidence; // confidence
161  int param3 = iterationsCount; // number maximum iterations
162 
163  Mat _local_model(3, 2, CV_64FC1);
164  Mat _mask_local_inliers(1, opoints.rows, CV_8UC1);
165 
166  // call Ransac
167  int result = createRANSACPointSetRegistrator(cb, model_points,
168  param1, param2, param3)->run(opoints, ipoints, _local_model, _mask_local_inliers);
169 
170  if( result > 0 )
171  {
172  std::vector<Point3d> opoints_inliers;
173  std::vector<Point2d> ipoints_inliers;
174  opoints.convertTo(opoints_inliers, CV_64F);
175  ipoints.convertTo(ipoints_inliers, CV_64F);
176 
177  const uchar* mask = _mask_local_inliers.ptr<uchar>();
178  int npoints1 = compressElems(&opoints_inliers[0], mask, 1, npoints);
179  compressElems(&ipoints_inliers[0], mask, 1, npoints);
180 
181  opoints_inliers.resize(npoints1);
182  ipoints_inliers.resize(npoints1);
183  result = solvePnP(opoints_inliers, ipoints_inliers, cameraMatrix,
184  distCoeffs, rvec, tvec, false, flags == CV_P3P ? CV_EPNP : flags) ? 1 : -1;
185  }
186 
187  if( result <= 0 || _local_model.rows <= 0)
188  {
189  rvec.copyTo(_rvec); // output rotation vector
190  tvec.copyTo(_tvec); // output translation vector
191 
192  if( _inliers.needed() )
193  _inliers.release();
194 
195  return false;
196  }
197  else
198  {
199  _local_model.col(0).copyTo(_rvec); // output rotation vector
200  _local_model.col(1).copyTo(_tvec); // output translation vector
201  }
202 
203  if(_inliers.needed())
204  {
205  Mat _local_inliers;
206  for (int i = 0; i < npoints; ++i)
207  {
208  if((int)_mask_local_inliers.at<uchar>(i) != 0) // inliers mask
209  _local_inliers.push_back(i); // output inliers vector
210  }
211  _local_inliers.copyTo(_inliers);
212  }
213  return true;
214 }
215 
216 int RANSACUpdateNumIters( double p, double ep, int modelPoints, int maxIters )
217 {
218  if( modelPoints <= 0 )
219  CV_Error( 0, "the number of model points should be positive" );
220 
221  p = MAX(p, 0.);
222  p = MIN(p, 1.);
223  ep = MAX(ep, 0.);
224  ep = MIN(ep, 1.);
225 
226  // avoid inf's & nan's
227  double num = MAX(1. - p, DBL_MIN);
228  double denom = 1. - std::pow(1. - ep, modelPoints);
229  if( denom < DBL_MIN )
230  return 0;
231 
232  num = std::log(num);
233  denom = std::log(denom);
234 
235  return denom >= 0 || -num >= maxIters*(-denom) ? maxIters : cvRound(num/denom);
236 }
237 
239 {
240 public:
241  RANSACPointSetRegistrator(const Ptr<PointSetRegistrator::Callback>& _cb=Ptr<PointSetRegistrator::Callback>(),
242  int _modelPoints=0, double _threshold=0, double _confidence=0.99, int _maxIters=1000)
243  : cb(_cb), modelPoints(_modelPoints), threshold(_threshold), confidence(_confidence), maxIters(_maxIters)
244  {
245  checkPartialSubsets = false;
246  }
247 
248  int findInliers( const Mat& m1, const Mat& m2, const Mat& model, Mat& err, Mat& mask, double thresh ) const
249  {
250  cb->computeError( m1, m2, model, err );
251  mask.create(err.size(), CV_8U);
252 
253  CV_Assert( err.isContinuous() && err.type() == CV_32F && mask.isContinuous() && mask.type() == CV_8U);
254  const float* errptr = err.ptr<float>();
255  uchar* maskptr = mask.ptr<uchar>();
256  float t = (float)(thresh*thresh);
257  int i, n = (int)err.total(), nz = 0;
258  for( i = 0; i < n; i++ )
259  {
260  int f = errptr[i] <= t;
261  maskptr[i] = (uchar)f;
262  nz += f;
263  }
264  return nz;
265  }
266 
267  bool getSubset( const Mat& m1, const Mat& m2,
268  Mat& ms1, Mat& ms2, RNG& rng,
269  int maxAttempts=1000 ) const
270  {
271  cv::AutoBuffer<int> _idx(modelPoints);
272  int* idx = _idx;
273  int i = 0, j, k, iters = 0;
274  int esz1 = (int)m1.elemSize(), esz2 = (int)m2.elemSize();
275  int d1 = m1.channels() > 1 ? m1.channels() : m1.cols;
276  int d2 = m2.channels() > 1 ? m2.channels() : m2.cols;
277  int count = m1.checkVector(d1), count2 = m2.checkVector(d2);
278  const int *m1ptr = m1.ptr<int>(), *m2ptr = m2.ptr<int>();
279 
280  ms1.create(modelPoints, 1, CV_MAKETYPE(m1.depth(), d1));
281  ms2.create(modelPoints, 1, CV_MAKETYPE(m2.depth(), d2));
282 
283  int *ms1ptr = ms1.ptr<int>(), *ms2ptr = ms2.ptr<int>();
284 
285  CV_Assert( count >= modelPoints && count == count2 );
286  CV_Assert( (esz1 % sizeof(int)) == 0 && (esz2 % sizeof(int)) == 0 );
287  esz1 /= sizeof(int);
288  esz2 /= sizeof(int);
289 
290  for(; iters < maxAttempts; iters++)
291  {
292  for( i = 0; i < modelPoints && iters < maxAttempts; )
293  {
294  int idx_i = 0;
295  for(;;)
296  {
297  idx_i = idx[i] = rng.uniform(0, count);
298  for( j = 0; j < i; j++ )
299  if( idx_i == idx[j] )
300  break;
301  if( j == i )
302  break;
303  }
304  for( k = 0; k < esz1; k++ )
305  ms1ptr[i*esz1 + k] = m1ptr[idx_i*esz1 + k];
306  for( k = 0; k < esz2; k++ )
307  ms2ptr[i*esz2 + k] = m2ptr[idx_i*esz2 + k];
308  if( checkPartialSubsets && !cb->checkSubset( ms1, ms2, i+1 ))
309  {
310  // we may have selected some bad points;
311  // so, let's remove some of them randomly
312  i = rng.uniform(0, i+1);
313  iters++;
314  continue;
315  }
316  i++;
317  }
318  if( !checkPartialSubsets && i == modelPoints && !cb->checkSubset(ms1, ms2, i))
319  continue;
320  break;
321  }
322 
323  return i == modelPoints && iters < maxAttempts;
324  }
325 
326  bool run(InputArray _m1, InputArray _m2, OutputArray _model, OutputArray _mask) const
327  {
328  bool result = false;
329  Mat m1 = _m1.getMat(), m2 = _m2.getMat();
330  Mat err, mask, model, bestModel, ms1, ms2;
331 
332  int iter, niters = MAX(maxIters, 1);
333  int d1 = m1.channels() > 1 ? m1.channels() : m1.cols;
334  int d2 = m2.channels() > 1 ? m2.channels() : m2.cols;
335  int count = m1.checkVector(d1), count2 = m2.checkVector(d2), maxGoodCount = 0;
336 
337  RNG rng((uint64)-1);
338 
339  CV_Assert( cb );
340  CV_Assert( confidence > 0 && confidence < 1 );
341 
342  CV_Assert( count >= 0 && count2 == count );
343  if( count < modelPoints )
344  return false;
345 
346  Mat bestMask0, bestMask;
347 
348  if( _mask.needed() )
349  {
350  _mask.create(count, 1, CV_8U, -1, true);
351  bestMask0 = bestMask = _mask.getMat();
352  CV_Assert( (bestMask.cols == 1 || bestMask.rows == 1) && (int)bestMask.total() == count );
353  }
354  else
355  {
356  bestMask.create(count, 1, CV_8U);
357  bestMask0 = bestMask;
358  }
359 
360  if( count == modelPoints )
361  {
362  if( cb->runKernel(m1, m2, bestModel) <= 0 )
363  return false;
364  bestModel.copyTo(_model);
365  bestMask.setTo(Scalar::all(1));
366  return true;
367  }
368 
369  for( iter = 0; iter < niters; iter++ )
370  {
371  int i, goodCount, nmodels;
372  if( count > modelPoints )
373  {
374  bool found = getSubset( m1, m2, ms1, ms2, rng, 10000 );
375  if( !found )
376  {
377  if( iter == 0 )
378  return false;
379  break;
380  }
381  }
382 
383  nmodels = cb->runKernel( ms1, ms2, model );
384  if( nmodels <= 0 )
385  continue;
386  CV_Assert( model.rows % nmodels == 0 );
387  Size modelSize(model.cols, model.rows/nmodels);
388 
389  for( i = 0; i < nmodels; i++ )
390  {
391  Mat model_i = model.rowRange( i*modelSize.height, (i+1)*modelSize.height );
392  goodCount = findInliers( m1, m2, model_i, err, mask, threshold );
393 
394  if( goodCount > MAX(maxGoodCount, modelPoints-1) )
395  {
396  std::swap(mask, bestMask);
397  model_i.copyTo(bestModel);
398  maxGoodCount = goodCount;
399  niters = RANSACUpdateNumIters( confidence, (double)(count - goodCount)/count, modelPoints, niters );
400  }
401  }
402  }
403 
404  if( maxGoodCount > 0 )
405  {
406  if( bestMask.data != bestMask0.data )
407  {
408  if( bestMask.size() == bestMask0.size() )
409  bestMask.copyTo(bestMask0);
410  else
411  transpose(bestMask, bestMask0);
412  }
413  bestModel.copyTo(_model);
414  result = true;
415  }
416  else
417  _model.release();
418 
419  return result;
420  }
421 
422  void setCallback(const Ptr<PointSetRegistrator::Callback>& _cb) { cb = _cb; }
423 
424  Ptr<PointSetRegistrator::Callback> cb;
427  double threshold;
428  double confidence;
429  int maxIters;
430 };
431 
433 {
434 public:
435  LMeDSPointSetRegistrator(const Ptr<PointSetRegistrator::Callback>& _cb=Ptr<PointSetRegistrator::Callback>(),
436  int _modelPoints=0, double _confidence=0.99, int _maxIters=1000)
437  : RANSACPointSetRegistrator(_cb, _modelPoints, 0, _confidence, _maxIters) {}
438 
439  bool run(InputArray _m1, InputArray _m2, OutputArray _model, OutputArray _mask) const
440  {
441  const double outlierRatio = 0.45;
442  bool result = false;
443  Mat m1 = _m1.getMat(), m2 = _m2.getMat();
444  Mat ms1, ms2, err, errf, model, bestModel, mask, mask0;
445 
446  int d1 = m1.channels() > 1 ? m1.channels() : m1.cols;
447  int d2 = m2.channels() > 1 ? m2.channels() : m2.cols;
448  int count = m1.checkVector(d1), count2 = m2.checkVector(d2);
449  double minMedian = DBL_MAX, sigma;
450 
451  RNG rng((uint64)-1);
452 
453  CV_Assert( cb );
454  CV_Assert( confidence > 0 && confidence < 1 );
455 
456  CV_Assert( count >= 0 && count2 == count );
457  if( count < modelPoints )
458  return false;
459 
460  if( _mask.needed() )
461  {
462  _mask.create(count, 1, CV_8U, -1, true);
463  mask0 = mask = _mask.getMat();
464  CV_Assert( (mask.cols == 1 || mask.rows == 1) && (int)mask.total() == count );
465  }
466 
467  if( count == modelPoints )
468  {
469  if( cb->runKernel(m1, m2, bestModel) <= 0 )
470  return false;
471  bestModel.copyTo(_model);
472  mask.setTo(Scalar::all(1));
473  return true;
474  }
475 
476  int iter, niters = RANSACUpdateNumIters(confidence, outlierRatio, modelPoints, maxIters);
477  niters = MAX(niters, 3);
478 
479  for( iter = 0; iter < niters; iter++ )
480  {
481  int i, nmodels;
482  if( count > modelPoints )
483  {
484  bool found = getSubset( m1, m2, ms1, ms2, rng );
485  if( !found )
486  {
487  if( iter == 0 )
488  return false;
489  break;
490  }
491  }
492 
493  nmodels = cb->runKernel( ms1, ms2, model );
494  if( nmodels <= 0 )
495  continue;
496 
497  CV_Assert( model.rows % nmodels == 0 );
498  Size modelSize(model.cols, model.rows/nmodels);
499 
500  for( i = 0; i < nmodels; i++ )
501  {
502  Mat model_i = model.rowRange( i*modelSize.height, (i+1)*modelSize.height );
503  cb->computeError( m1, m2, model_i, err );
504  if( err.depth() != CV_32F )
505  err.convertTo(errf, CV_32F);
506  else
507  errf = err;
508  CV_Assert( errf.isContinuous() && errf.type() == CV_32F && (int)errf.total() == count );
509  std::sort(errf.ptr<int>(), errf.ptr<int>() + count);
510 
511  double median = count % 2 != 0 ?
512  errf.at<float>(count/2) : (errf.at<float>(count/2-1) + errf.at<float>(count/2))*0.5;
513 
514  if( median < minMedian )
515  {
516  minMedian = median;
517  model_i.copyTo(bestModel);
518  }
519  }
520  }
521 
522  if( minMedian < DBL_MAX )
523  {
524  sigma = 2.5*1.4826*(1 + 5./(count - modelPoints))*std::sqrt(minMedian);
525  sigma = MAX( sigma, 0.001 );
526 
527  count = findInliers( m1, m2, bestModel, err, mask, sigma );
528  if( _mask.needed() && mask0.data != mask.data )
529  {
530  if( mask0.size() == mask.size() )
531  mask.copyTo(mask0);
532  else
533  transpose(mask, mask0);
534  }
535  bestModel.copyTo(_model);
536  result = count >= modelPoints;
537  }
538  else
539  _model.release();
540 
541  return result;
542  }
543 
544 };
545 
546 Ptr<PointSetRegistrator> createRANSACPointSetRegistrator(const Ptr<PointSetRegistrator::Callback>& _cb,
547  int _modelPoints, double _threshold,
548  double _confidence, int _maxIters)
549 {
550  return Ptr<PointSetRegistrator>(
551  new RANSACPointSetRegistrator(_cb, _modelPoints, _threshold, _confidence, _maxIters));
552 }
553 
554 
555 Ptr<PointSetRegistrator> createLMeDSPointSetRegistrator(const Ptr<PointSetRegistrator::Callback>& _cb,
556  int _modelPoints, double _confidence, int _maxIters)
557 {
558  return Ptr<PointSetRegistrator>(
559  new LMeDSPointSetRegistrator(_cb, _modelPoints, _confidence, _maxIters));
560 }
561 
562 
564 {
565 public:
566  int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const
567  {
568  Mat m1 = _m1.getMat(), m2 = _m2.getMat();
569  const Point3f* from = m1.ptr<Point3f>();
570  const Point3f* to = m2.ptr<Point3f>();
571 
572  const int N = 12;
573  double buf[N*N + N + N];
574  Mat A(N, N, CV_64F, &buf[0]);
575  Mat B(N, 1, CV_64F, &buf[0] + N*N);
576  Mat X(N, 1, CV_64F, &buf[0] + N*N + N);
577  double* Adata = A.ptr<double>();
578  double* Bdata = B.ptr<double>();
579  A = Scalar::all(0);
580 
581  for( int i = 0; i < (N/3); i++ )
582  {
583  Bdata[i*3] = to[i].x;
584  Bdata[i*3+1] = to[i].y;
585  Bdata[i*3+2] = to[i].z;
586 
587  double *aptr = Adata + i*3*N;
588  for(int k = 0; k < 3; ++k)
589  {
590  aptr[0] = from[i].x;
591  aptr[1] = from[i].y;
592  aptr[2] = from[i].z;
593  aptr[3] = 1.0;
594  aptr += 16;
595  }
596  }
597 
598  solve(A, B, X, DECOMP_SVD);
599  X.reshape(1, 3).copyTo(_model);
600 
601  return 1;
602  }
603 
604  void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const
605  {
606  Mat m1 = _m1.getMat(), m2 = _m2.getMat(), model = _model.getMat();
607  const Point3f* from = m1.ptr<Point3f>();
608  const Point3f* to = m2.ptr<Point3f>();
609  const double* F = model.ptr<double>();
610 
611  int count = m1.checkVector(3);
612  CV_Assert( count > 0 );
613 
614  _err.create(count, 1, CV_32F);
615  Mat err = _err.getMat();
616  float* errptr = err.ptr<float>();
617 
618  for(int i = 0; i < count; i++ )
619  {
620  const Point3f& f = from[i];
621  const Point3f& t = to[i];
622 
623  double a = F[0]*f.x + F[1]*f.y + F[ 2]*f.z + F[ 3] - t.x;
624  double b = F[4]*f.x + F[5]*f.y + F[ 6]*f.z + F[ 7] - t.y;
625  double c = F[8]*f.x + F[9]*f.y + F[10]*f.z + F[11] - t.z;
626 
627  errptr[i] = (float)std::sqrt(a*a + b*b + c*c);
628  }
629  }
630 
631  bool checkSubset( InputArray _ms1, InputArray _ms2, int count ) const
632  {
633  const float threshold = 0.996f;
634  Mat ms1 = _ms1.getMat(), ms2 = _ms2.getMat();
635 
636  for( int inp = 1; inp <= 2; inp++ )
637  {
638  int j, k, i = count - 1;
639  const Mat* msi = inp == 1 ? &ms1 : &ms2;
640  const Point3f* ptr = msi->ptr<Point3f>();
641 
642  CV_Assert( count <= msi->rows );
643 
644  // check that the i-th selected point does not belong
645  // to a line connecting some previously selected points
646  for(j = 0; j < i; ++j)
647  {
648  Point3f d1 = ptr[j] - ptr[i];
649  float n1 = d1.x*d1.x + d1.y*d1.y;
650 
651  for(k = 0; k < j; ++k)
652  {
653  Point3f d2 = ptr[k] - ptr[i];
654  float denom = (d2.x*d2.x + d2.y*d2.y)*n1;
655  float num = d1.x*d2.x + d1.y*d2.y;
656 
657  if( num*num > threshold*threshold*denom )
658  return false;
659  }
660  }
661  }
662  return true;
663  }
664 };
665 
666 }
GLM_FUNC_DECL genType log(genType const &x)
GLM_FUNC_DECL genIType mask(genIType const &count)
int RANSACUpdateNumIters(double p, double ep, int modelPoints, int maxIters)
Definition: solvepnp.cpp:216
Ptr< PointSetRegistrator > createRANSACPointSetRegistrator(const Ptr< PointSetRegistrator::Callback > &_cb, int _modelPoints, double _threshold, double _confidence, int _maxIters)
Definition: solvepnp.cpp:546
bool getSubset(const Mat &m1, const Mat &m2, Mat &ms1, Mat &ms2, RNG &rng, int maxAttempts=1000) const
Definition: solvepnp.cpp:267
Ptr< PointSetRegistrator::Callback > cb
Definition: solvepnp.cpp:424
f
#define MAX(A, B)
Definition: sqlite3.c:8236
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
bool run(InputArray _m1, InputArray _m2, OutputArray _model, OutputArray _mask) const
Definition: solvepnp.cpp:326
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:241
GLM_FUNC_DECL bool all(vecType< bool, P > const &v)
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
int runKernel(InputArray _m1, InputArray _m2, OutputArray _model) const
Definition: solvepnp.cpp:566
#define MIN(A, B)
Definition: sqlite3.c:8235
void computeError(InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err) const
Definition: solvepnp.cpp:604
bool run(InputArray _m1, InputArray _m2, OutputArray _model, OutputArray _mask) const
Definition: solvepnp.cpp:439
int findInliers(const Mat &m1, const Mat &m2, const Mat &model, Mat &err, Mat &mask, double thresh) const
Definition: solvepnp.cpp:248
int compressElems(T *ptr, const uchar *mask, int mstep, int count)
Definition: solvepnp.h:141
bool checkSubset(InputArray _ms1, InputArray _ms2, int count) const
Definition: solvepnp.cpp:631
unsigned long long uint64
Definition: type_int.hpp:65
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
GLM_FUNC_DECL genType pow(genType const &base, genType const &exponent)
void computeError(InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err) const
Definition: solvepnp.cpp:79
int runKernel(InputArray _m1, InputArray _m2, OutputArray _model) const
Definition: solvepnp.cpp:63
LMeDSPointSetRegistrator(const Ptr< PointSetRegistrator::Callback > &_cb=Ptr< PointSetRegistrator::Callback >(), int _modelPoints=0, double _confidence=0.99, int _maxIters=1000)
Definition: solvepnp.cpp:435
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:422
Ptr< PointSetRegistrator > createLMeDSPointSetRegistrator(const Ptr< PointSetRegistrator::Callback > &_cb, int _modelPoints, double _confidence, int _maxIters)
Definition: solvepnp.cpp:555


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:35:00