57 bool _useExtrinsicGuess=
false,
Mat _rvec=
Mat(),
Mat _tvec=
Mat() )
58 : cameraMatrix(_cameraMatrix), distCoeffs(_distCoeffs), flags(_flags), useExtrinsicGuess(_useExtrinsicGuess),
59 rvec(_rvec), tvec(_tvec) {}
63 int runKernel( InputArray _m1, InputArray _m2, OutputArray _model )
const
65 Mat opoints = _m1.getMat(), ipoints = _m2.getMat();
67 bool correspondence = solvePnP( _m1, _m2, cameraMatrix, distCoeffs,
68 rvec, tvec, useExtrinsicGuess, flags );
71 hconcat(rvec, tvec, _local_model);
72 _local_model.copyTo(_model);
74 return correspondence;
79 void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err )
const
82 Mat opoints = _m1.getMat(), ipoints = _m2.getMat(),
model = _model.getMat();
84 int i,
count = opoints.checkVector(3);
90 projectPoints(opoints, _rvec, _tvec, cameraMatrix, distCoeffs, projpoints);
92 const Point2f* ipoints_ptr = ipoints.ptr<Point2f>();
93 const Point2f* projpoints_ptr = projpoints.ptr<Point2f>();
95 _err.create(
count, 1, CV_32FC1);
96 float* err = _err.getMat().ptr<
float>();
99 err[
i] = (
float)norm( ipoints_ptr[
i] - projpoints_ptr[
i] );
113 InputArray _cameraMatrix, InputArray _distCoeffs,
114 OutputArray _rvec, OutputArray _tvec,
bool useExtrinsicGuess,
115 int iterationsCount,
float reprojectionError,
double confidence,
116 OutputArray _inliers,
int flags)
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);
125 if( ipoints0.depth() == CV_64F || !ipoints0.isContinuous() )
126 ipoints0.convertTo(ipoints, CV_32F);
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)) );
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);
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();
144 int model_points = 6;
145 int ransac_kernel_method = CV_EPNP;
150 ransac_kernel_method = CV_P3P;
153 Ptr<PointSetRegistrator::Callback> cb;
154 cb = Ptr<PnPRansacCallback>(
new PnPRansacCallback( cameraMatrix, distCoeffs, ransac_kernel_method, useExtrinsicGuess, rvec, tvec));
156 double param1 = reprojectionError;
157 double param2 = confidence;
158 int param3 = iterationsCount;
160 Mat _local_model(3, 2, CV_64FC1);
161 Mat _mask_local_inliers(1, opoints.rows, CV_8UC1);
165 param1, param2, param3)->run(opoints, ipoints, _local_model, _mask_local_inliers);
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);
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;
184 if(
result <= 0 || _local_model.rows <= 0)
189 if( _inliers.needed() )
196 _local_model.col(0).copyTo(_rvec);
197 _local_model.col(1).copyTo(_tvec);
200 if(_inliers.needed())
203 for (
int i = 0;
i < npoints; ++
i)
205 if((
int)_mask_local_inliers.at<
uchar>(
i) != 0)
206 _local_inliers.push_back(
i);
208 _local_inliers.copyTo(_inliers);
215 if( modelPoints <= 0 )
216 CV_Error( 0,
"the number of model points should be positive" );
224 double num =
MAX(1. -
p, DBL_MIN);
225 double denom = 1. -
std::pow(1. - ep, modelPoints);
226 if( denom < DBL_MIN )
232 return denom >= 0 || -num >= maxIters*(-denom) ? maxIters : cvRound(num/denom);
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)
242 checkPartialSubsets =
false;
248 mask.create(err.size(), CV_8U);
250 CV_Assert( err.isContinuous() && err.type() == CV_32F &&
mask.isContinuous() &&
mask.type() == CV_8U);
251 const float* errptr = err.ptr<
float>();
253 float t = (
float)(thresh*thresh);
254 int i,
n = (
int)err.total(), nz = 0;
255 for(
i = 0;
i <
n;
i++ )
257 int f = errptr[
i] <=
t;
265 Mat& ms1,
Mat& ms2, RNG& rng,
266 int maxAttempts=1000 )
const
268 cv::AutoBuffer<int> _idx(modelPoints);
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;
275 const int *m1ptr =
m1.ptr<
int>(), *m2ptr =
m2.ptr<
int>();
277 ms1.create(modelPoints, 1, CV_MAKETYPE(
m1.depth(),
d1));
278 ms2.create(modelPoints, 1, CV_MAKETYPE(
m2.depth(),
d2));
280 int *ms1ptr = ms1.ptr<
int>(), *ms2ptr = ms2.ptr<
int>();
282 CV_Assert(
count >= modelPoints &&
count == count2 );
283 CV_Assert( (esz1 %
sizeof(
int)) == 0 && (esz2 %
sizeof(
int)) == 0 );
287 for(; iters < maxAttempts; iters++)
289 for(
i = 0;
i < modelPoints && iters < maxAttempts; )
294 idx_i = idx[
i] = rng.uniform(0,
count);
295 for(
j = 0;
j <
i;
j++ )
296 if( idx_i == idx[
j] )
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 ))
309 i = rng.uniform(0,
i+1);
315 if( !checkPartialSubsets &&
i == modelPoints && !cb->checkSubset(ms1, ms2,
i))
320 return i == modelPoints && iters < maxAttempts;
323 bool run(InputArray _m1, InputArray _m2, OutputArray _model, OutputArray _mask)
const
326 Mat m1 = _m1.getMat(),
m2 = _m2.getMat();
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;
337 CV_Assert( confidence > 0 && confidence < 1 );
340 if(
count < modelPoints )
343 Mat bestMask0, bestMask;
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 );
353 bestMask.create(
count, 1, CV_8U);
354 bestMask0 = bestMask;
357 if(
count == modelPoints )
359 if( cb->runKernel(
m1,
m2, bestModel) <= 0 )
361 bestModel.copyTo(_model);
368 int i, goodCount, nmodels;
369 if(
count > modelPoints )
371 bool found = getSubset(
m1,
m2, ms1, ms2, rng, 10000 );
380 nmodels = cb->runKernel( ms1, ms2,
model );
383 CV_Assert(
model.rows % nmodels == 0 );
384 Size modelSize(
model.cols,
model.rows/nmodels);
386 for(
i = 0;
i < nmodels;
i++ )
388 Mat model_i =
model.rowRange(
i*modelSize.height, (
i+1)*modelSize.height );
389 goodCount = findInliers(
m1,
m2, model_i, err,
mask, threshold );
391 if( goodCount >
MAX(maxGoodCount, modelPoints-1) )
394 model_i.copyTo(bestModel);
395 maxGoodCount = goodCount;
401 if( maxGoodCount > 0 )
403 if( bestMask.data != bestMask0.data )
405 if( bestMask.size() == bestMask0.size() )
406 bestMask.copyTo(bestMask0);
408 transpose(bestMask, bestMask0);
410 bestModel.copyTo(_model);
419 void setCallback(
const Ptr<PointSetRegistrator::Callback>& _cb) { cb = _cb; }
421 Ptr<PointSetRegistrator::Callback>
cb;
433 int _modelPoints=0,
double _confidence=0.99,
int _maxIters=1000)
436 bool run(InputArray _m1, InputArray _m2, OutputArray _model, OutputArray _mask)
const
438 const double outlierRatio = 0.45;
440 Mat m1 = _m1.getMat(),
m2 = _m2.getMat();
443 int d1 =
m1.channels() > 1 ?
m1.channels() :
m1.cols;
444 int d2 =
m2.channels() > 1 ?
m2.channels() :
m2.cols;
446 double minMedian = DBL_MAX, sigma;
451 CV_Assert( confidence > 0 && confidence < 1 );
454 if(
count < modelPoints )
459 _mask.create(
count, 1, CV_8U, -1,
true);
460 mask0 =
mask = _mask.getMat();
464 if(
count == modelPoints )
466 if( cb->runKernel(
m1,
m2, bestModel) <= 0 )
468 bestModel.copyTo(_model);
474 niters =
MAX(niters, 3);
479 if(
count > modelPoints )
481 bool found = getSubset(
m1,
m2, ms1, ms2, rng );
490 nmodels = cb->runKernel( ms1, ms2,
model );
494 CV_Assert(
model.rows % nmodels == 0 );
495 Size modelSize(
model.cols,
model.rows/nmodels);
497 for(
i = 0;
i < nmodels;
i++ )
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);
505 CV_Assert( errf.isContinuous() && errf.type() == CV_32F && (
int)errf.total() ==
count );
506 std::sort(errf.ptr<
int>(), errf.ptr<
int>() +
count);
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;
511 if( median < minMedian )
514 model_i.copyTo(bestModel);
519 if( minMedian < DBL_MAX )
521 sigma = 2.5*1.4826*(1 + 5./(
count - modelPoints))*
std::sqrt(minMedian);
522 sigma =
MAX( sigma, 0.001 );
525 if( _mask.needed() && mask0.data !=
mask.data )
527 if( mask0.size() ==
mask.size() )
530 transpose(
mask, mask0);
532 bestModel.copyTo(_model);
544 int _modelPoints,
double _threshold,
545 double _confidence,
int _maxIters)
547 return Ptr<PointSetRegistrator>(
553 int _modelPoints,
double _confidence,
int _maxIters)
555 return Ptr<PointSetRegistrator>(
563 int runKernel( InputArray _m1, InputArray _m2, OutputArray _model )
const
565 Mat m1 = _m1.getMat(),
m2 = _m2.getMat();
566 const Point3f* from =
m1.ptr<Point3f>();
567 const Point3f* to =
m2.ptr<Point3f>();
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>();
578 for(
int i = 0;
i < (
N/3);
i++ )
580 Bdata[
i*3] = to[
i].x;
581 Bdata[
i*3+1] = to[
i].y;
582 Bdata[
i*3+2] = to[
i].z;
584 double *aptr = Adata +
i*3*
N;
585 for(
int k = 0; k < 3; ++k)
595 solve(
A,
B,
X, DECOMP_SVD);
596 X.reshape(1, 3).copyTo(_model);
601 void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err )
const
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>();
609 CV_Assert(
count > 0 );
611 _err.create(
count, 1, CV_32F);
612 Mat err = _err.getMat();
613 float* errptr = err.ptr<
float>();
617 const Point3f&
f = from[
i];
618 const Point3f&
t = to[
i];
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;
628 bool checkSubset( InputArray _ms1, InputArray _ms2,
int count )
const
630 const float threshold = 0.996f;
631 Mat ms1 = _ms1.getMat(), ms2 = _ms2.getMat();
633 for(
int inp = 1; inp <= 2; inp++ )
636 const Mat* msi = inp == 1 ? &ms1 : &ms2;
637 const Point3f* ptr = msi->ptr<Point3f>();
639 CV_Assert( count <= msi->
rows );
643 for(
j = 0;
j <
i; ++
j)
645 Point3f
d1 = ptr[
j] - ptr[
i];
648 for(k = 0; k <
j; ++k)
650 Point3f
d2 = ptr[k] - ptr[
i];
654 if( num*num > threshold*threshold*denom )