1 // This file is part of OpenCV project.
2 // It is subject to the license terms in the LICENSE file found in the top-level directory
3 // of this distribution and at http://opencv.org/license.html.
4
5 #include "../precomp.hpp"
6 #include "../usac.hpp"
7 #include <atomic>
8
9 namespace cv { namespace usac {
10 int mergePoints (InputArray pts1_, InputArray pts2_, Mat &pts, bool ispnp);
11 void setParameters (int flag, Ptr<Model> ¶ms, EstimationMethod estimator, double thr,
12 int max_iters, double conf, bool mask_needed);
13
14 class RansacOutputImpl : public RansacOutput {
15 private:
16 Mat model;
17 // vector of number_inliers size
18 std::vector<int> inliers;
19 // vector of points size, true if inlier, false-outlier
20 std::vector<bool> inliers_mask;
21 // vector of points size, value of i-th index corresponds to error of i-th point if i is inlier.
22 std::vector<double> errors;
23 // the best found score of RANSAC
24 double score;
25
26 int seconds, milliseconds, microseconds;
27 int time_mcs, number_inliers, number_estimated_models, number_good_models;
28 int number_iterations; // number of iterations of main RANSAC
29 public:
RansacOutputImpl(const Mat & model_,const std::vector<bool> & inliers_mask_,int time_mcs_,double score_,int number_inliers_,int number_iterations_,int number_estimated_models_,int number_good_models_)30 RansacOutputImpl (const Mat &model_, const std::vector<bool> &inliers_mask_,
31 int time_mcs_, double score_, int number_inliers_, int number_iterations_,
32 int number_estimated_models_, int number_good_models_) {
33
34 model_.copyTo(model);
35 inliers_mask = inliers_mask_;
36 time_mcs = time_mcs_;
37 score = score_;
38 number_inliers = number_inliers_;
39 number_iterations = number_iterations_;
40 number_estimated_models = number_estimated_models_;
41 number_good_models = number_good_models_;
42 microseconds = time_mcs % 1000;
43 milliseconds = ((time_mcs - microseconds)/1000) % 1000;
44 seconds = ((time_mcs - 1000*milliseconds - microseconds)/(1000*1000)) % 60;
45 }
46
47 /*
48 * Return inliers' indices.
49 * size of vector = number of inliers
50 */
getInliers()51 const std::vector<int> &getInliers() override {
52 if (inliers.empty()) {
53 inliers.reserve(inliers_mask.size());
54 int pt_cnt = 0;
55 for (bool is_inlier : inliers_mask) {
56 if (is_inlier)
57 inliers.emplace_back(pt_cnt);
58 pt_cnt++;
59 }
60 }
61 return inliers;
62 }
63
64 // Return inliers mask. Vector of points size. 1-inlier, 0-outlier.
getInliersMask() const65 const std::vector<bool> &getInliersMask() const override { return inliers_mask; }
66
getTimeMicroSeconds() const67 int getTimeMicroSeconds() const override {return time_mcs; }
getTimeMicroSeconds1() const68 int getTimeMicroSeconds1() const override {return microseconds; }
getTimeMilliSeconds2() const69 int getTimeMilliSeconds2() const override {return milliseconds; }
getTimeSeconds3() const70 int getTimeSeconds3() const override {return seconds; }
getNumberOfInliers() const71 int getNumberOfInliers() const override { return number_inliers; }
getNumberOfMainIterations() const72 int getNumberOfMainIterations() const override { return number_iterations; }
getNumberOfGoodModels() const73 int getNumberOfGoodModels () const override { return number_good_models; }
getNumberOfEstimatedModels() const74 int getNumberOfEstimatedModels () const override { return number_estimated_models; }
getModel() const75 const Mat &getModel() const override { return model; }
76 };
77
create(const Mat & model_,const std::vector<bool> & inliers_mask_,int time_mcs_,double score_,int number_inliers_,int number_iterations_,int number_estimated_models_,int number_good_models_)78 Ptr<RansacOutput> RansacOutput::create(const Mat &model_, const std::vector<bool> &inliers_mask_,
79 int time_mcs_, double score_, int number_inliers_, int number_iterations_,
80 int number_estimated_models_, int number_good_models_) {
81 return makePtr<RansacOutputImpl>(model_, inliers_mask_, time_mcs_, score_, number_inliers_,
82 number_iterations_, number_estimated_models_, number_good_models_);
83 }
84
85 class Ransac {
86 protected:
87 const Ptr<const Model> params;
88 const Ptr<const Estimator> _estimator;
89 const Ptr<Quality> _quality;
90 const Ptr<Sampler> _sampler;
91 const Ptr<TerminationCriteria> _termination_criteria;
92 const Ptr<ModelVerifier> _model_verifier;
93 const Ptr<Degeneracy> _degeneracy;
94 const Ptr<LocalOptimization> _local_optimization;
95 const Ptr<FinalModelPolisher> model_polisher;
96
97 const int points_size, state;
98 const bool parallel;
99 public:
100
Ransac(const Ptr<const Model> & params_,int points_size_,const Ptr<const Estimator> & estimator_,const Ptr<Quality> & quality_,const Ptr<Sampler> & sampler_,const Ptr<TerminationCriteria> & termination_criteria_,const Ptr<ModelVerifier> & model_verifier_,const Ptr<Degeneracy> & degeneracy_,const Ptr<LocalOptimization> & local_optimization_,const Ptr<FinalModelPolisher> & model_polisher_,bool parallel_=false,int state_=0)101 Ransac (const Ptr<const Model> ¶ms_, int points_size_, const Ptr<const Estimator> &estimator_, const Ptr<Quality> &quality_,
102 const Ptr<Sampler> &sampler_, const Ptr<TerminationCriteria> &termination_criteria_,
103 const Ptr<ModelVerifier> &model_verifier_, const Ptr<Degeneracy> °eneracy_,
104 const Ptr<LocalOptimization> &local_optimization_, const Ptr<FinalModelPolisher> &model_polisher_,
105 bool parallel_=false, int state_ = 0) :
106
107 params (params_), _estimator (estimator_), _quality (quality_), _sampler (sampler_),
108 _termination_criteria (termination_criteria_), _model_verifier (model_verifier_),
109 _degeneracy (degeneracy_), _local_optimization (local_optimization_),
110 model_polisher (model_polisher_), points_size (points_size_), state(state_),
111 parallel(parallel_) {}
112
run(Ptr<RansacOutput> & ransac_output)113 bool run(Ptr<RansacOutput> &ransac_output) {
114 if (points_size < params->getSampleSize())
115 return false;
116
117 const auto begin_time = std::chrono::steady_clock::now();
118
119 // check if LO
120 const bool LO = params->getLO() != LocalOptimMethod::LOCAL_OPTIM_NULL;
121 const bool is_magsac = params->getLO() == LocalOptimMethod::LOCAL_OPTIM_SIGMA;
122 const int max_hyp_test_before_ver = params->getMaxNumHypothesisToTestBeforeRejection();
123 const int repeat_magsac = 10, max_iters_before_LO = params->getMaxItersBeforeLO();
124 Score best_score;
125 Mat best_model;
126 int final_iters;
127
128 if (! parallel) {
129 auto update_best = [&] (const Mat &new_model, const Score &new_score) {
130 best_score = new_score;
131 // remember best model
132 new_model.copyTo(best_model);
133 // update quality and verifier to save evaluation time of a model
134 _quality->setBestScore(best_score.score);
135 // update verifier
136 _model_verifier->update(best_score.inlier_number);
137 // update upper bound of iterations
138 return _termination_criteria->update(best_model, best_score.inlier_number);
139 };
140 bool was_LO_run = false;
141 Mat non_degenerate_model, lo_model;
142 Score current_score, lo_score, non_denegenerate_model_score;
143
144 // reallocate memory for models
145 std::vector<Mat> models(_estimator->getMaxNumSolutions());
146
147 // allocate memory for sample
148 std::vector<int> sample(_estimator->getMinimalSampleSize());
149 int iters = 0, max_iters = params->getMaxIters();
150 for (; iters < max_iters; iters++) {
151 _sampler->generateSample(sample);
152 const int number_of_models = _estimator->estimateModels(sample, models);
153
154 for (int i = 0; i < number_of_models; i++) {
155 if (iters < max_hyp_test_before_ver) {
156 current_score = _quality->getScore(models[i]);
157 } else {
158 if (is_magsac && iters % repeat_magsac == 0) {
159 if (!_local_optimization->refineModel
160 (models[i], best_score, models[i], current_score))
161 continue;
162 } else if (_model_verifier->isModelGood(models[i])) {
163 if (!_model_verifier->getScore(current_score)) {
164 if (_model_verifier->hasErrors())
165 current_score = _quality->getScore(_model_verifier->getErrors());
166 else current_score = _quality->getScore(models[i]);
167 }
168 } else continue;
169 }
170
171 if (current_score.isBetter(best_score)) {
172 if (_degeneracy->recoverIfDegenerate(sample, models[i],
173 non_degenerate_model, non_denegenerate_model_score)) {
174 // check if best non degenerate model is better than so far the best model
175 if (non_denegenerate_model_score.isBetter(best_score))
176 max_iters = update_best(non_degenerate_model, non_denegenerate_model_score);
177 else continue;
178 } else max_iters = update_best(models[i], current_score);
179
180 if (LO && iters >= max_iters_before_LO) {
181 // do magsac if it wasn't already run
182 if (is_magsac && iters % repeat_magsac == 0 && iters >= max_hyp_test_before_ver) continue; // magsac has already run
183 was_LO_run = true;
184 // update model by Local optimization
185 if (_local_optimization->refineModel
186 (best_model, best_score, lo_model, lo_score)) {
187 if (lo_score.isBetter(best_score)){
188 max_iters = update_best(lo_model, lo_score);
189 }
190 }
191 }
192 if (iters > max_iters)
193 break;
194 } // end of if so far the best score
195 } // end loop of number of models
196 if (LO && !was_LO_run && iters >= max_iters_before_LO) {
197 was_LO_run = true;
198 if (_local_optimization->refineModel(best_model, best_score, lo_model, lo_score))
199 if (lo_score.isBetter(best_score)){
200 max_iters = update_best(lo_model, lo_score);
201 }
202 }
203 } // end main while loop
204
205 final_iters = iters;
206 } else {
207 const int MAX_THREADS = getNumThreads();
208 const bool is_prosac = params->getSampler() == SamplingMethod::SAMPLING_PROSAC;
209
210 std::atomic_bool success(false);
211 std::atomic_int num_hypothesis_tested(0);
212 std::atomic_int thread_cnt(0);
213 std::vector<Score> best_scores(MAX_THREADS);
214 std::vector<Mat> best_models(MAX_THREADS);
215
216 Mutex mutex; // only for prosac
217
218 ///////////////////////////////////////////////////////////////////////////////////////////////////////
219 parallel_for_(Range(0, MAX_THREADS), [&](const Range & /*range*/) {
220 if (!success) { // cover all if not success to avoid thread creating new variables
221 const int thread_rng_id = thread_cnt++;
222 int thread_state = state + 10*thread_rng_id;
223
224 Ptr<Estimator> estimator = _estimator->clone();
225 Ptr<Degeneracy> degeneracy = _degeneracy->clone(thread_state++);
226 Ptr<Quality> quality = _quality->clone();
227 Ptr<ModelVerifier> model_verifier = _model_verifier->clone(thread_state++); // update verifier
228 Ptr<LocalOptimization> local_optimization;
229 if (LO)
230 local_optimization = _local_optimization->clone(thread_state++);
231 Ptr<TerminationCriteria> termination_criteria = _termination_criteria->clone();
232 Ptr<Sampler> sampler;
233 if (!is_prosac)
234 sampler = _sampler->clone(thread_state);
235
236 Mat best_model_thread, non_degenerate_model, lo_model;
237 Score best_score_thread, current_score, non_denegenerate_model_score, lo_score,
238 best_score_all_threads;
239 std::vector<int> sample(estimator->getMinimalSampleSize());
240 std::vector<Mat> models(estimator->getMaxNumSolutions());
241 int iters, max_iters = params->getMaxIters();
242 auto update_best = [&] (const Score &new_score, const Mat &new_model) {
243 // copy new score to best score
244 best_score_thread = new_score;
245 best_scores[thread_rng_id] = best_score_thread;
246 // remember best model
247 new_model.copyTo(best_model_thread);
248 best_model_thread.copyTo(best_models[thread_rng_id]);
249 best_score_all_threads = best_score_thread;
250 // update upper bound of iterations
251 return termination_criteria->update
252 (best_model_thread, best_score_thread.inlier_number);
253 };
254
255 bool was_LO_run = false;
256 for (iters = 0; iters < max_iters && !success; iters++) {
257 success = num_hypothesis_tested++ > max_iters;
258
259 if (iters % 10) {
260 // Synchronize threads. just to speed verification of model.
261 int best_thread_idx = thread_rng_id;
262 bool updated = false;
263 for (int t = 0; t < MAX_THREADS; t++) {
264 if (best_scores[t].isBetter(best_score_all_threads)) {
265 best_score_all_threads = best_scores[t];
266 updated = true;
267 best_thread_idx = t;
268 }
269 }
270 if (updated && best_thread_idx != thread_rng_id) {
271 quality->setBestScore(best_score_all_threads.score);
272 model_verifier->update(best_score_all_threads.inlier_number);
273 }
274 }
275
276 if (is_prosac) {
277 // use global sampler
278 mutex.lock();
279 _sampler->generateSample(sample);
280 mutex.unlock();
281 } else sampler->generateSample(sample); // use local sampler
282
283 const int number_of_models = estimator->estimateModels(sample, models);
284 for (int i = 0; i < number_of_models; i++) {
285 if (iters < max_hyp_test_before_ver) {
286 current_score = quality->getScore(models[i]);
287 } else {
288 if (is_magsac && iters % repeat_magsac == 0) {
289 if (local_optimization && !local_optimization->refineModel
290 (models[i], best_score_thread, models[i], current_score))
291 continue;
292 } else if (model_verifier->isModelGood(models[i])) {
293 if (!model_verifier->getScore(current_score)) {
294 if (model_verifier->hasErrors())
295 current_score = quality->getScore(model_verifier->getErrors());
296 else current_score = quality->getScore(models[i]);
297 }
298 } else continue;
299 }
300
301 if (current_score.isBetter(best_score_all_threads)) {
302 if (degeneracy->recoverIfDegenerate(sample, models[i],
303 non_degenerate_model, non_denegenerate_model_score)) {
304 // check if best non degenerate model is better than so far the best model
305 if (non_denegenerate_model_score.isBetter(best_score_thread))
306 max_iters = update_best(non_denegenerate_model_score, non_degenerate_model);
307 else continue;
308 } else
309 max_iters = update_best(current_score, models[i]);
310
311 if (LO && iters >= max_iters_before_LO) {
312 // do magsac if it wasn't already run
313 if (is_magsac && iters % repeat_magsac == 0 && iters >= max_hyp_test_before_ver) continue;
314 was_LO_run = true;
315 // update model by Local optimizaion
316 if (local_optimization->refineModel
317 (best_model_thread, best_score_thread, lo_model, lo_score))
318 if (lo_score.isBetter(best_score_thread)) {
319 max_iters = update_best(lo_score, lo_model);
320 }
321 }
322 if (num_hypothesis_tested > max_iters) {
323 success = true; break;
324 }
325 } // end of if so far the best score
326 } // end loop of number of models
327 if (LO && !was_LO_run && iters >= max_iters_before_LO) {
328 was_LO_run = true;
329 if (_local_optimization->refineModel(best_model, best_score, lo_model, lo_score))
330 if (lo_score.isBetter(best_score)){
331 max_iters = update_best(lo_score, lo_model);
332 }
333 }
334 } // end of loop over iters
335 }}); // end parallel
336 ///////////////////////////////////////////////////////////////////////////////////////////////////////
337 // find best model from all threads' models
338 best_score = best_scores[0];
339 int best_thread_idx = 0;
340 for (int i = 1; i < MAX_THREADS; i++) {
341 if (best_scores[i].isBetter(best_score)) {
342 best_score = best_scores[i];
343 best_thread_idx = i;
344 }
345 }
346 best_model = best_models[best_thread_idx];
347 final_iters = num_hypothesis_tested;
348 }
349
350 if (best_model.empty())
351 return false;
352
353 // polish final model
354 if (params->getFinalPolisher() != PolishingMethod::NonePolisher) {
355 Mat polished_model;
356 Score polisher_score;
357 if (model_polisher->polishSoFarTheBestModel(best_model, best_score,
358 polished_model, polisher_score))
359 if (polisher_score.isBetter(best_score)) {
360 best_score = polisher_score;
361 polished_model.copyTo(best_model);
362 }
363 }
364 // ================= here is ending ransac main implementation ===========================
365 std::vector<bool> inliers_mask;
366 if (params->isMaskRequired()) {
367 inliers_mask = std::vector<bool>(points_size);
368 // get final inliers from the best model
369 _quality->getInliers(best_model, inliers_mask);
370 }
371 // Store results
372 ransac_output = RansacOutput::create(best_model, inliers_mask,
373 static_cast<int>(std::chrono::duration_cast<std::chrono::microseconds>
374 (std::chrono::steady_clock::now() - begin_time).count()), best_score.score,
375 best_score.inlier_number, final_iters, -1, -1);
376 return true;
377 }
378 };
379
380 /*
381 * pts1, pts2 are matrices either N x a, N x b or a x N or b x N, where N > a and N > b
382 * pts1 are image points, if pnp pts2 are object points otherwise - image points as well.
383 * output is matrix of size N x (a + b)
384 * return points_size = N
385 */
mergePoints(InputArray pts1_,InputArray pts2_,Mat & pts,bool ispnp)386 int mergePoints (InputArray pts1_, InputArray pts2_, Mat &pts, bool ispnp) {
387 Mat pts1 = pts1_.getMat(), pts2 = pts2_.getMat();
388 auto convertPoints = [] (Mat &points, int pt_dim) {
389 points.convertTo(points, CV_32F); // convert points to have float precision
390 if (points.channels() > 1)
391 points = points.reshape(1, (int)points.total()); // convert point to have 1 channel
392 if (points.rows < points.cols)
393 transpose(points, points); // transpose so points will be in rows
394 CV_CheckGE(points.cols, pt_dim, "Invalid dimension of point");
395 if (points.cols != pt_dim) // in case when image points are 3D convert them to 2D
396 points = points.colRange(0, pt_dim);
397 };
398
399 convertPoints(pts1, 2); // pts1 are always image points
400 convertPoints(pts2, ispnp ? 3 : 2); // for PnP points are 3D
401
402 // points are of size [Nx2 Nx2] = Nx4 for H, F, E
403 // points are of size [Nx2 Nx3] = Nx5 for PnP
404 hconcat(pts1, pts2, pts);
405 return pts.rows;
406 }
407
saveMask(OutputArray mask,const std::vector<bool> & inliers_mask)408 void saveMask (OutputArray mask, const std::vector<bool> &inliers_mask) {
409 if (mask.needed()) {
410 const int points_size = (int) inliers_mask.size();
411 Mat tmp_mask(points_size, 1, CV_8U);
412 auto * maskptr = tmp_mask.ptr<uchar>();
413 for (int i = 0; i < points_size; i++)
414 maskptr[i] = (uchar) inliers_mask[i];
415 tmp_mask.copyTo(mask);
416 }
417 }
setParameters(Ptr<Model> & params,EstimationMethod estimator,const UsacParams & usac_params,bool mask_needed)418 void setParameters (Ptr<Model> ¶ms, EstimationMethod estimator, const UsacParams &usac_params,
419 bool mask_needed) {
420 params = Model::create(usac_params.threshold, estimator, usac_params.sampler,
421 usac_params.confidence, usac_params.maxIterations, usac_params.score);
422 params->setLocalOptimization(usac_params.loMethod);
423 params->setLOSampleSize(usac_params.loSampleSize);
424 params->setLOIterations(usac_params.loIterations);
425 params->setParallel(usac_params.isParallel);
426 params->setNeighborsType(usac_params.neighborsSearch);
427 params->setRandomGeneratorState(usac_params.randomGeneratorState);
428 params->maskRequired(mask_needed);
429 }
430
setParameters(int flag,Ptr<Model> & params,EstimationMethod estimator,double thr,int max_iters,double conf,bool mask_needed)431 void setParameters (int flag, Ptr<Model> ¶ms, EstimationMethod estimator, double thr,
432 int max_iters, double conf, bool mask_needed) {
433 switch (flag) {
434 case USAC_DEFAULT:
435 params = Model::create(thr, estimator, SamplingMethod::SAMPLING_UNIFORM, conf, max_iters,
436 ScoreMethod::SCORE_METHOD_MSAC);
437 params->setLocalOptimization(LocalOptimMethod ::LOCAL_OPTIM_INNER_AND_ITER_LO);
438 break;
439 case USAC_MAGSAC:
440 params = Model::create(thr, estimator, SamplingMethod::SAMPLING_UNIFORM, conf, max_iters,
441 ScoreMethod::SCORE_METHOD_MAGSAC);
442 params->setLocalOptimization(LocalOptimMethod ::LOCAL_OPTIM_SIGMA);
443 params->setLOSampleSize(params->isHomography() ? 75 : 50);
444 params->setLOIterations(params->isHomography() ? 15 : 10);
445 break;
446 case USAC_PARALLEL:
447 params = Model::create(thr, estimator, SamplingMethod::SAMPLING_UNIFORM, conf, max_iters,
448 ScoreMethod::SCORE_METHOD_MSAC);
449 params->setParallel(true);
450 params->setLocalOptimization(LocalOptimMethod ::LOCAL_OPTIM_INNER_LO);
451 break;
452 case USAC_ACCURATE:
453 params = Model::create(thr, estimator, SamplingMethod::SAMPLING_UNIFORM, conf, max_iters,
454 ScoreMethod::SCORE_METHOD_MSAC);
455 params->setLocalOptimization(LocalOptimMethod ::LOCAL_OPTIM_GC);
456 params->setLOSampleSize(20);
457 params->setLOIterations(25);
458 break;
459 case USAC_FAST:
460 params = Model::create(thr, estimator, SamplingMethod::SAMPLING_UNIFORM, conf, max_iters,
461 ScoreMethod::SCORE_METHOD_MSAC);
462 params->setLocalOptimization(LocalOptimMethod ::LOCAL_OPTIM_INNER_AND_ITER_LO);
463 params->setLOIterations(5);
464 params->setLOIterativeIters(3);
465 break;
466 case USAC_PROSAC:
467 params = Model::create(thr, estimator, SamplingMethod::SAMPLING_PROSAC, conf, max_iters,
468 ScoreMethod::SCORE_METHOD_MSAC);
469 params->setLocalOptimization(LocalOptimMethod ::LOCAL_OPTIM_INNER_LO);
470 break;
471 case USAC_FM_8PTS:
472 params = Model::create(thr, EstimationMethod::Fundamental8,SamplingMethod::SAMPLING_UNIFORM,
473 conf, max_iters,ScoreMethod::SCORE_METHOD_MSAC);
474 params->setLocalOptimization(LocalOptimMethod ::LOCAL_OPTIM_INNER_LO);
475 break;
476 default: CV_Error(cv::Error::StsBadFlag, "Incorrect flag for USAC!");
477 }
478 // do not do too many iterations for PnP
479 if (estimator == EstimationMethod::P3P) {
480 if (params->getLOInnerMaxIters() > 15)
481 params->setLOIterations(15);
482 params->setLOIterativeIters(0);
483 }
484
485 params->maskRequired(mask_needed);
486 }
487
findHomography(InputArray srcPoints,InputArray dstPoints,int method,double thr,OutputArray mask,const int max_iters,const double confidence)488 Mat findHomography (InputArray srcPoints, InputArray dstPoints, int method, double thr,
489 OutputArray mask, const int max_iters, const double confidence) {
490 Ptr<Model> params;
491 setParameters(method, params, EstimationMethod::Homography, thr, max_iters, confidence, mask.needed());
492 Ptr<RansacOutput> ransac_output;
493 if (run(params, srcPoints, dstPoints, params->getRandomGeneratorState(),
494 ransac_output, noArray(), noArray(), noArray(), noArray())) {
495 saveMask(mask, ransac_output->getInliersMask());
496 return ransac_output->getModel() / ransac_output->getModel().at<double>(2,2);
497 }
498 if (mask.needed()){
499 mask.create(std::max(srcPoints.getMat().rows, srcPoints.getMat().cols), 1, CV_8U);
500 mask.setTo(Scalar::all(0));
501 }
502 return Mat();
503 }
504
findFundamentalMat(InputArray points1,InputArray points2,int method,double thr,double confidence,int max_iters,OutputArray mask)505 Mat findFundamentalMat( InputArray points1, InputArray points2, int method, double thr,
506 double confidence, int max_iters, OutputArray mask ) {
507 Ptr<Model> params;
508 setParameters(method, params, EstimationMethod::Fundamental, thr, max_iters, confidence, mask.needed());
509 Ptr<RansacOutput> ransac_output;
510 if (run(params, points1, points2, params->getRandomGeneratorState(),
511 ransac_output, noArray(), noArray(), noArray(), noArray())) {
512 saveMask(mask, ransac_output->getInliersMask());
513 return ransac_output->getModel();
514 }
515 if (mask.needed()){
516 mask.create(std::max(points1.getMat().rows, points1.getMat().cols), 1, CV_8U);
517 mask.setTo(Scalar::all(0));
518 }
519 return Mat();
520 }
521
findEssentialMat(InputArray points1,InputArray points2,InputArray cameraMatrix1,int method,double prob,double thr,OutputArray mask)522 Mat findEssentialMat (InputArray points1, InputArray points2, InputArray cameraMatrix1,
523 int method, double prob, double thr, OutputArray mask) {
524 Ptr<Model> params;
525 setParameters(method, params, EstimationMethod::Essential, thr, 1000, prob, mask.needed());
526 Ptr<RansacOutput> ransac_output;
527 if (run(params, points1, points2, params->getRandomGeneratorState(),
528 ransac_output, cameraMatrix1, cameraMatrix1, noArray(), noArray())) {
529 saveMask(mask, ransac_output->getInliersMask());
530 return ransac_output->getModel();
531 }
532 if (mask.needed()){
533 mask.create(std::max(points1.getMat().rows, points1.getMat().cols), 1, CV_8U);
534 mask.setTo(Scalar::all(0));
535 }
536 return Mat();
537 }
538
solvePnPRansac(InputArray objectPoints,InputArray imagePoints,InputArray cameraMatrix,InputArray distCoeffs,OutputArray rvec,OutputArray tvec,bool,int max_iters,float thr,double conf,OutputArray inliers,int method)539 bool solvePnPRansac( InputArray objectPoints, InputArray imagePoints,
540 InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec,
541 bool /*useExtrinsicGuess*/, int max_iters, float thr, double conf,
542 OutputArray inliers, int method) {
543 Ptr<Model> params;
544 setParameters(method, params, cameraMatrix.empty() ? EstimationMethod ::P6P : EstimationMethod ::P3P,
545 thr, max_iters, conf, inliers.needed());
546 Ptr<RansacOutput> ransac_output;
547 if (run(params, imagePoints, objectPoints, params->getRandomGeneratorState(),
548 ransac_output, cameraMatrix, noArray(), distCoeffs, noArray())) {
549 if (inliers.needed()) {
550 const auto &inliers_mask = ransac_output->getInliersMask();
551 Mat inliers_;
552 for (int i = 0; i < (int)inliers_mask.size(); i++)
553 if (inliers_mask[i])
554 inliers_.push_back(i);
555 inliers_.copyTo(inliers);
556 }
557 const Mat &model = ransac_output->getModel();
558 model.col(0).copyTo(rvec);
559 model.col(1).copyTo(tvec);
560 return true;
561 }
562 return false;
563 }
564
estimateAffine2D(InputArray from,InputArray to,OutputArray mask,int method,double thr,int max_iters,double conf,int)565 Mat estimateAffine2D(InputArray from, InputArray to, OutputArray mask, int method,
566 double thr, int max_iters, double conf, int /*refineIters*/) {
567 Ptr<Model> params;
568 setParameters(method, params, EstimationMethod ::Affine, thr, max_iters, conf, mask.needed());
569 Ptr<RansacOutput> ransac_output;
570 if (run(params, from, to, params->getRandomGeneratorState(),
571 ransac_output, noArray(), noArray(), noArray(), noArray())) {
572 saveMask(mask, ransac_output->getInliersMask());
573 return ransac_output->getModel().rowRange(0,2);
574 }
575 if (mask.needed()){
576 mask.create(std::max(from.getMat().rows, from.getMat().cols), 1, CV_8U);
577 mask.setTo(Scalar::all(0));
578 }
579 return Mat();
580 }
581
582 class ModelImpl : public Model {
583 private:
584 // main parameters:
585 double threshold, confidence;
586 int sample_size, max_iterations;
587
588 EstimationMethod estimator;
589 SamplingMethod sampler;
590 ScoreMethod score;
591
592 // for neighborhood graph
593 int k_nearest_neighbors = 8;//, flann_search_params = 5, num_kd_trees = 1; // for FLANN
594 int cell_size = 50; // pixels, for grid neighbors searching
595 int radius = 30; // pixels, for radius-search neighborhood graph
596 NeighborSearchMethod neighborsType = NeighborSearchMethod::NEIGH_GRID;
597
598 // Local Optimization parameters
599 LocalOptimMethod lo = LocalOptimMethod ::LOCAL_OPTIM_INNER_AND_ITER_LO;
600 int lo_sample_size=16, lo_inner_iterations=15, lo_iterative_iterations=8,
601 lo_thr_multiplier=15, lo_iter_sample_size = 30;
602
603 // Graph cut parameters
604 const double spatial_coherence_term = 0.975;
605
606 // apply polisher for final RANSAC model
607 PolishingMethod polisher = PolishingMethod ::LSQPolisher;
608
609 // preemptive verification test
610 VerificationMethod verifier = VerificationMethod ::SprtVerifier;
611 const int max_hypothesis_test_before_verification = 15;
612
613 // sprt parameters
614 // lower bound estimate is 1% of inliers
615 double sprt_eps = 0.01, sprt_delta = 0.008, avg_num_models, time_for_model_est;
616
617 // estimator error
618 ErrorMetric est_error;
619
620 // progressive napsac
621 double relax_coef = 0.1;
622 // for building neighborhood graphs
623 const std::vector<int> grid_cell_number = {16, 8, 4, 2};
624
625 //for final least squares polisher
626 int final_lsq_iters = 3;
627
628 bool need_mask = true, is_parallel = false;
629 int random_generator_state = 0;
630 const int max_iters_before_LO = 100;
631
632 // magsac parameters:
633 int DoF = 2;
634 double sigma_quantile = 3.04, upper_incomplete_of_sigma_quantile = 0.00419,
635 lower_incomplete_of_sigma_quantile = 0.8629, C = 0.5, maximum_thr = 7.5;
636 public:
ModelImpl(double threshold_,EstimationMethod estimator_,SamplingMethod sampler_,double confidence_=0.95,int max_iterations_=5000,ScoreMethod score_=ScoreMethod::SCORE_METHOD_MSAC)637 ModelImpl (double threshold_, EstimationMethod estimator_, SamplingMethod sampler_, double confidence_=0.95,
638 int max_iterations_=5000, ScoreMethod score_ =ScoreMethod::SCORE_METHOD_MSAC) {
639 estimator = estimator_;
640 sampler = sampler_;
641 confidence = confidence_;
642 max_iterations = max_iterations_;
643 score = score_;
644
645 switch (estimator_) {
646 // time for model estimation is basically a ratio of time need to estimate a model to
647 // time needed to verify if a point is consistent with this model
648 case (EstimationMethod::Affine):
649 avg_num_models = 1; time_for_model_est = 50;
650 sample_size = 3; est_error = ErrorMetric ::FORW_REPR_ERR; break;
651 case (EstimationMethod::Homography):
652 avg_num_models = 1; time_for_model_est = 150;
653 sample_size = 4; est_error = ErrorMetric ::FORW_REPR_ERR; break;
654 case (EstimationMethod::Fundamental):
655 avg_num_models = 2.38; time_for_model_est = 180; maximum_thr = 2.5;
656 sample_size = 7; est_error = ErrorMetric ::SAMPSON_ERR; break;
657 case (EstimationMethod::Fundamental8):
658 avg_num_models = 1; time_for_model_est = 100; maximum_thr = 2.5;
659 sample_size = 8; est_error = ErrorMetric ::SAMPSON_ERR; break;
660 case (EstimationMethod::Essential):
661 avg_num_models = 3.93; time_for_model_est = 1000; maximum_thr = 2.5;
662 sample_size = 5; est_error = ErrorMetric ::SGD_ERR; break;
663 case (EstimationMethod::P3P):
664 avg_num_models = 1.38; time_for_model_est = 800;
665 sample_size = 3; est_error = ErrorMetric ::RERPOJ; break;
666 case (EstimationMethod::P6P):
667 avg_num_models = 1; time_for_model_est = 300;
668 sample_size = 6; est_error = ErrorMetric ::RERPOJ; break;
669 default: CV_Error(cv::Error::StsNotImplemented, "Estimator has not implemented yet!");
670 }
671
672 if (estimator_ == EstimationMethod::P3P || estimator_ == EstimationMethod::P6P) {
673 neighborsType = NeighborSearchMethod::NEIGH_FLANN_KNN;
674 k_nearest_neighbors = 2;
675 }
676 if (estimator == EstimationMethod::Fundamental || estimator == EstimationMethod::Essential) {
677 lo_sample_size = 21;
678 lo_thr_multiplier = 10;
679 }
680 if (estimator == EstimationMethod::Homography)
681 maximum_thr = 8.;
682 threshold = threshold_;
683 }
setVerifier(VerificationMethod verifier_)684 void setVerifier (VerificationMethod verifier_) override { verifier = verifier_; }
setPolisher(PolishingMethod polisher_)685 void setPolisher (PolishingMethod polisher_) override { polisher = polisher_; }
setParallel(bool is_parallel_)686 void setParallel (bool is_parallel_) override { is_parallel = is_parallel_; }
setError(ErrorMetric error_)687 void setError (ErrorMetric error_) override { est_error = error_; }
setLocalOptimization(LocalOptimMethod lo_)688 void setLocalOptimization (LocalOptimMethod lo_) override { lo = lo_; }
setKNearestNeighhbors(int knn_)689 void setKNearestNeighhbors (int knn_) override { k_nearest_neighbors = knn_; }
setNeighborsType(NeighborSearchMethod neighbors)690 void setNeighborsType (NeighborSearchMethod neighbors) override { neighborsType = neighbors; }
setCellSize(int cell_size_)691 void setCellSize (int cell_size_) override { cell_size = cell_size_; }
setLOIterations(int iters)692 void setLOIterations (int iters) override { lo_inner_iterations = iters; }
setLOIterativeIters(int iters)693 void setLOIterativeIters (int iters) override {lo_iterative_iterations = iters; }
setLOSampleSize(int lo_sample_size_)694 void setLOSampleSize (int lo_sample_size_) override { lo_sample_size = lo_sample_size_; }
setThresholdMultiplierLO(double thr_mult)695 void setThresholdMultiplierLO (double thr_mult) override { lo_thr_multiplier = (int) round(thr_mult); }
maskRequired(bool need_mask_)696 void maskRequired (bool need_mask_) override { need_mask = need_mask_; }
setRandomGeneratorState(int state)697 void setRandomGeneratorState (int state) override { random_generator_state = state; }
isMaskRequired() const698 bool isMaskRequired () const override { return need_mask; }
getNeighborsSearch() const699 NeighborSearchMethod getNeighborsSearch () const override { return neighborsType; }
getKNN() const700 int getKNN () const override { return k_nearest_neighbors; }
getError() const701 ErrorMetric getError () const override { return est_error; }
getEstimator() const702 EstimationMethod getEstimator () const override { return estimator; }
getSampleSize() const703 int getSampleSize () const override { return sample_size; }
getFinalLSQIterations() const704 int getFinalLSQIterations () const override { return final_lsq_iters; }
getDegreesOfFreedom() const705 int getDegreesOfFreedom () const override { return DoF; }
getSigmaQuantile() const706 double getSigmaQuantile () const override { return sigma_quantile; }
getUpperIncompleteOfSigmaQuantile() const707 double getUpperIncompleteOfSigmaQuantile () const override {
708 return upper_incomplete_of_sigma_quantile;
709 }
getLowerIncompleteOfSigmaQuantile() const710 double getLowerIncompleteOfSigmaQuantile () const override {
711 return lower_incomplete_of_sigma_quantile;
712 }
getC() const713 double getC () const override { return C; }
getMaximumThreshold() const714 double getMaximumThreshold () const override { return maximum_thr; }
getGraphCutSpatialCoherenceTerm() const715 double getGraphCutSpatialCoherenceTerm () const override { return spatial_coherence_term; }
getLOSampleSize() const716 int getLOSampleSize () const override { return lo_sample_size; }
getMaxNumHypothesisToTestBeforeRejection() const717 int getMaxNumHypothesisToTestBeforeRejection() const override {
718 return max_hypothesis_test_before_verification;
719 }
getFinalPolisher() const720 PolishingMethod getFinalPolisher () const override { return polisher; }
getLOThresholdMultiplier() const721 int getLOThresholdMultiplier() const override { return lo_thr_multiplier; }
getLOIterativeSampleSize() const722 int getLOIterativeSampleSize() const override { return lo_iter_sample_size; }
getLOIterativeMaxIters() const723 int getLOIterativeMaxIters() const override { return lo_iterative_iterations; }
getLOInnerMaxIters() const724 int getLOInnerMaxIters() const override { return lo_inner_iterations; }
getLO() const725 LocalOptimMethod getLO () const override { return lo; }
getScore() const726 ScoreMethod getScore () const override { return score; }
getMaxIters() const727 int getMaxIters () const override { return max_iterations; }
getConfidence() const728 double getConfidence () const override { return confidence; }
getThreshold() const729 double getThreshold () const override { return threshold; }
getVerifier() const730 VerificationMethod getVerifier () const override { return verifier; }
getSampler() const731 SamplingMethod getSampler () const override { return sampler; }
getRandomGeneratorState() const732 int getRandomGeneratorState () const override { return random_generator_state; }
getMaxItersBeforeLO() const733 int getMaxItersBeforeLO () const override { return max_iters_before_LO; }
getSPRTdelta() const734 double getSPRTdelta () const override { return sprt_delta; }
getSPRTepsilon() const735 double getSPRTepsilon () const override { return sprt_eps; }
getSPRTavgNumModels() const736 double getSPRTavgNumModels () const override { return avg_num_models; }
getCellSize() const737 int getCellSize () const override { return cell_size; }
getGraphRadius() const738 int getGraphRadius() const override { return radius; }
getTimeForModelEstimation() const739 double getTimeForModelEstimation () const override { return time_for_model_est; }
getRelaxCoef() const740 double getRelaxCoef () const override { return relax_coef; }
getGridCellNumber() const741 const std::vector<int> &getGridCellNumber () const override { return grid_cell_number; }
isParallel() const742 bool isParallel () const override { return is_parallel; }
isFundamental() const743 bool isFundamental () const override {
744 return estimator == EstimationMethod ::Fundamental ||
745 estimator == EstimationMethod ::Fundamental8;
746 }
isHomography() const747 bool isHomography () const override { return estimator == EstimationMethod ::Homography; }
isEssential() const748 bool isEssential () const override { return estimator == EstimationMethod ::Essential; }
isPnP() const749 bool isPnP() const override {
750 return estimator == EstimationMethod ::P3P || estimator == EstimationMethod ::P6P;
751 }
752 };
753
create(double threshold_,EstimationMethod estimator_,SamplingMethod sampler_,double confidence_,int max_iterations_,ScoreMethod score_)754 Ptr<Model> Model::create(double threshold_, EstimationMethod estimator_, SamplingMethod sampler_,
755 double confidence_, int max_iterations_, ScoreMethod score_) {
756 return makePtr<ModelImpl>(threshold_, estimator_, sampler_, confidence_,
757 max_iterations_, score_);
758 }
759
run(const Ptr<const Model> & params,InputArray points1,InputArray points2,int state,Ptr<RansacOutput> & ransac_output,InputArray K1_,InputArray K2_,InputArray dist_coeff1,InputArray dist_coeff2)760 bool run (const Ptr<const Model> ¶ms, InputArray points1, InputArray points2, int state,
761 Ptr<RansacOutput> &ransac_output, InputArray K1_, InputArray K2_,
762 InputArray dist_coeff1, InputArray dist_coeff2) {
763 Ptr<Error> error;
764 Ptr<Estimator> estimator;
765 Ptr<NeighborhoodGraph> graph;
766 Ptr<Degeneracy> degeneracy;
767 Ptr<Quality> quality;
768 Ptr<ModelVerifier> verifier;
769 Ptr<Sampler> sampler;
770 Ptr<RandomGenerator> lo_sampler;
771 Ptr<TerminationCriteria> termination;
772 Ptr<LocalOptimization> lo;
773 Ptr<FinalModelPolisher> polisher;
774 Ptr<MinimalSolver> min_solver;
775 Ptr<NonMinimalSolver> non_min_solver;
776
777 Mat points, K1, K2, calib_points, undist_points1, undist_points2;
778 int points_size;
779 double threshold = params->getThreshold(), max_thr = params->getMaximumThreshold();
780 const int min_sample_size = params->getSampleSize();
781 if (params->isPnP()) {
782 if (! K1_.empty()) {
783 K1 = K1_.getMat(); K1.convertTo(K1, CV_64F);
784 if (! dist_coeff1.empty()) {
785 // undistortPoints also calibrate points using K
786 if (points1.isContinuous())
787 undistortPoints(points1, undist_points1, K1_, dist_coeff1);
788 else undistortPoints(points1.getMat().clone(), undist_points1, K1_, dist_coeff1);
789 points_size = mergePoints(undist_points1, points2, points, true);
790 Utils::normalizeAndDecalibPointsPnP (K1, points, calib_points);
791 } else {
792 points_size = mergePoints(points1, points2, points, true);
793 Utils::calibrateAndNormalizePointsPnP(K1, points, calib_points);
794 }
795 } else
796 points_size = mergePoints(points1, points2, points, true);
797 } else {
798 if (params->isEssential()) {
799 CV_CheckEQ((int)(!K1_.empty() && !K2_.empty()), 1, "Intrinsic matrix must not be empty!");
800 K1 = K1_.getMat(); K1.convertTo(K1, CV_64F);
801 K2 = K2_.getMat(); K2.convertTo(K2, CV_64F);
802 if (! dist_coeff1.empty() || ! dist_coeff2.empty()) {
803 // undistortPoints also calibrate points using K
804 if (points1.isContinuous())
805 undistortPoints(points1, undist_points1, K1_, dist_coeff1);
806 else undistortPoints(points1.getMat().clone(), undist_points1, K1_, dist_coeff1);
807 if (points2.isContinuous())
808 undistortPoints(points2, undist_points2, K2_, dist_coeff2);
809 else undistortPoints(points2.getMat().clone(), undist_points2, K2_, dist_coeff2);
810 points_size = mergePoints(undist_points1, undist_points2, calib_points, false);
811 } else {
812 points_size = mergePoints(points1, points2, points, false);
813 Utils::calibratePoints(K1, K2, points, calib_points);
814 }
815 threshold = Utils::getCalibratedThreshold(threshold, K1, K2);
816 max_thr = Utils::getCalibratedThreshold(max_thr, K1, K2);
817 } else
818 points_size = mergePoints(points1, points2, points, false);
819 }
820
821 // Since error function output squared error distance, so make
822 // threshold squared as well
823 threshold *= threshold;
824
825 if (params->getSampler() == SamplingMethod::SAMPLING_NAPSAC || params->getLO() == LocalOptimMethod::LOCAL_OPTIM_GC) {
826 if (params->getNeighborsSearch() == NeighborSearchMethod::NEIGH_GRID) {
827 graph = GridNeighborhoodGraph::create(points, points_size,
828 params->getCellSize(), params->getCellSize(),
829 params->getCellSize(), params->getCellSize(), 10);
830 } else if (params->getNeighborsSearch() == NeighborSearchMethod::NEIGH_FLANN_KNN) {
831 graph = FlannNeighborhoodGraph::create(points, points_size,params->getKNN(), false, 5, 1);
832 } else if (params->getNeighborsSearch() == NeighborSearchMethod::NEIGH_FLANN_RADIUS) {
833 graph = RadiusSearchNeighborhoodGraph::create(points, points_size,
834 params->getGraphRadius(), 5, 1);
835 } else CV_Error(cv::Error::StsNotImplemented, "Graph type is not implemented!");
836 }
837
838 std::vector<Ptr<NeighborhoodGraph>> layers;
839 if (params->getSampler() == SamplingMethod::SAMPLING_PROGRESSIVE_NAPSAC) {
840 CV_CheckEQ((int)params->isPnP(), 0, "ProgressiveNAPSAC for PnP is not implemented!");
841 const auto &cell_number_per_layer = params->getGridCellNumber();
842 layers.reserve(cell_number_per_layer.size());
843 const auto * const pts = (float *) points.data;
844 float img1_width = 0, img1_height = 0, img2_width = 0, img2_height = 0;
845 for (int i = 0; i < 4 * points_size; i += 4) {
846 if (pts[i ] > img1_width ) img1_width = pts[i ];
847 if (pts[i + 1] > img1_height) img1_height = pts[i + 1];
848 if (pts[i + 2] > img2_width ) img2_width = pts[i + 2];
849 if (pts[i + 3] > img2_height) img2_height = pts[i + 3];
850 }
851 // Create grid graphs (overlapping layes of given cell numbers)
852 for (int layer_idx = 0; layer_idx < (int)cell_number_per_layer.size(); layer_idx++) {
853 const int cell_number = cell_number_per_layer[layer_idx];
854 if (layer_idx > 0)
855 if (cell_number_per_layer[layer_idx-1] <= cell_number)
856 CV_Error(cv::Error::StsError, "Progressive NAPSAC sampler: "
857 "Cell number in layers must be in decreasing order!");
858 layers.emplace_back(GridNeighborhoodGraph::create(points, points_size,
859 (int)(img1_width / (float)cell_number), (int)(img1_height / (float)cell_number),
860 (int)(img2_width / (float)cell_number), (int)(img2_height / (float)cell_number), 10));
861 }
862 }
863
864 // update points by calibrated for Essential matrix after graph is calculated
865 if (params->isEssential()) {
866 points = calib_points;
867 // if maximum calibrated threshold significanlty differs threshold then set upper bound
868 if (max_thr > 10*threshold)
869 max_thr = sqrt(10*threshold); // max thr will be squared after
870 }
871 if (max_thr < threshold)
872 max_thr = threshold;
873
874 switch (params->getError()) {
875 case ErrorMetric::SYMM_REPR_ERR:
876 error = ReprojectionErrorSymmetric::create(points); break;
877 case ErrorMetric::FORW_REPR_ERR:
878 if (params->getEstimator() == EstimationMethod::Affine)
879 error = ReprojectionErrorAffine::create(points);
880 else error = ReprojectionErrorForward::create(points);
881 break;
882 case ErrorMetric::SAMPSON_ERR:
883 error = SampsonError::create(points); break;
884 case ErrorMetric::SGD_ERR:
885 error = SymmetricGeometricDistance::create(points); break;
886 case ErrorMetric::RERPOJ:
887 error = ReprojectionErrorPmatrix::create(points); break;
888 default: CV_Error(cv::Error::StsNotImplemented , "Error metric is not implemented!");
889 }
890
891 switch (params->getScore()) {
892 case ScoreMethod::SCORE_METHOD_RANSAC :
893 quality = RansacQuality::create(points_size, threshold, error); break;
894 case ScoreMethod::SCORE_METHOD_MSAC :
895 quality = MsacQuality::create(points_size, threshold, error); break;
896 case ScoreMethod::SCORE_METHOD_MAGSAC :
897 quality = MagsacQuality::create(max_thr, points_size, error,
898 threshold, params->getDegreesOfFreedom(), params->getSigmaQuantile(),
899 params->getUpperIncompleteOfSigmaQuantile(),
900 params->getLowerIncompleteOfSigmaQuantile(), params->getC()); break;
901 case ScoreMethod::SCORE_METHOD_LMEDS :
902 quality = LMedsQuality::create(points_size, threshold, error); break;
903 default: CV_Error(cv::Error::StsNotImplemented, "Score is not imeplemeted!");
904 }
905
906 if (params->isHomography()) {
907 degeneracy = HomographyDegeneracy::create(points);
908 min_solver = HomographyMinimalSolver4ptsGEM::create(points);
909 non_min_solver = HomographyNonMinimalSolver::create(points);
910 estimator = HomographyEstimator::create(min_solver, non_min_solver, degeneracy);
911 } else if (params->isFundamental()) {
912 degeneracy = FundamentalDegeneracy::create(state++, quality, points, min_sample_size, 5. /*sqr homogr thr*/);
913 if(min_sample_size == 7) min_solver = FundamentalMinimalSolver7pts::create(points);
914 else min_solver = FundamentalMinimalSolver8pts::create(points);
915 non_min_solver = FundamentalNonMinimalSolver::create(points);
916 estimator = FundamentalEstimator::create(min_solver, non_min_solver, degeneracy);
917 } else if (params->isEssential()) {
918 degeneracy = EssentialDegeneracy::create(points, min_sample_size);
919 min_solver = EssentialMinimalSolverStewenius5pts::create(points);
920 non_min_solver = EssentialNonMinimalSolver::create(points);
921 estimator = EssentialEstimator::create(min_solver, non_min_solver, degeneracy);
922 } else if (params->isPnP()) {
923 degeneracy = makePtr<Degeneracy>();
924 if (min_sample_size == 3) {
925 non_min_solver = DLSPnP::create(points, calib_points, K1);
926 min_solver = P3PSolver::create(points, calib_points, K1);
927 } else {
928 min_solver = PnPMinimalSolver6Pts::create(points);
929 non_min_solver = PnPNonMinimalSolver::create(points);
930 }
931 estimator = PnPEstimator::create(min_solver, non_min_solver);
932 } else if (params->getEstimator() == EstimationMethod::Affine) {
933 degeneracy = makePtr<Degeneracy>();
934 min_solver = AffineMinimalSolver::create(points);
935 non_min_solver = AffineNonMinimalSolver::create(points);
936 estimator = AffineEstimator::create(min_solver, non_min_solver);
937 } else CV_Error(cv::Error::StsNotImplemented, "Estimator not implemented!");
938
939 switch (params->getSampler()) {
940 case SamplingMethod::SAMPLING_UNIFORM:
941 sampler = UniformSampler::create(state++, min_sample_size, points_size); break;
942 case SamplingMethod::SAMPLING_PROSAC:
943 sampler = ProsacSampler::create(state++, points_size, min_sample_size, 200000); break;
944 case SamplingMethod::SAMPLING_PROGRESSIVE_NAPSAC:
945 sampler = ProgressiveNapsac::create(state++, points_size, min_sample_size, layers, 20); break;
946 case SamplingMethod::SAMPLING_NAPSAC:
947 sampler = NapsacSampler::create(state++, points_size, min_sample_size, graph); break;
948 default: CV_Error(cv::Error::StsNotImplemented, "Sampler is not implemented!");
949 }
950
951 switch (params->getVerifier()) {
952 case VerificationMethod::NullVerifier: verifier = ModelVerifier::create(); break;
953 case VerificationMethod::SprtVerifier:
954 verifier = SPRT::create(state++, error, points_size, params->getScore() == ScoreMethod ::SCORE_METHOD_MAGSAC ? max_thr : threshold,
955 params->getSPRTepsilon(), params->getSPRTdelta(), params->getTimeForModelEstimation(),
956 params->getSPRTavgNumModels(), params->getScore()); break;
957 default: CV_Error(cv::Error::StsNotImplemented, "Verifier is not imeplemented!");
958 }
959
960 if (params->getSampler() == SamplingMethod::SAMPLING_PROSAC) {
961 termination = ProsacTerminationCriteria::create(sampler.dynamicCast<ProsacSampler>(), error,
962 points_size, min_sample_size, params->getConfidence(),
963 params->getMaxIters(), 100, 0.05, 0.05, threshold);
964 } else if (params->getSampler() == SamplingMethod::SAMPLING_PROGRESSIVE_NAPSAC) {
965 if (params->getVerifier() == VerificationMethod::SprtVerifier)
966 termination = SPRTPNapsacTermination::create(((SPRT *)verifier.get())->getSPRTvector(),
967 params->getConfidence(), points_size, min_sample_size,
968 params->getMaxIters(), params->getRelaxCoef());
969 else
970 termination = StandardTerminationCriteria::create (params->getConfidence(),
971 points_size, min_sample_size, params->getMaxIters());
972 } else if (params->getVerifier() == VerificationMethod::SprtVerifier) {
973 termination = SPRTTermination::create(((SPRT *) verifier.get())->getSPRTvector(),
974 params->getConfidence(), points_size, min_sample_size, params->getMaxIters());
975 } else
976 termination = StandardTerminationCriteria::create
977 (params->getConfidence(), points_size, min_sample_size, params->getMaxIters());
978
979 if (params->getLO() != LocalOptimMethod::LOCAL_OPTIM_NULL) {
980 lo_sampler = UniformRandomGenerator::create(state++, points_size, params->getLOSampleSize());
981 switch (params->getLO()) {
982 case LocalOptimMethod::LOCAL_OPTIM_INNER_LO:
983 lo = InnerIterativeLocalOptimization::create(estimator, quality, lo_sampler,
984 points_size, threshold, false, params->getLOIterativeSampleSize(),
985 params->getLOInnerMaxIters(), params->getLOIterativeMaxIters(),
986 params->getLOThresholdMultiplier()); break;
987 case LocalOptimMethod::LOCAL_OPTIM_INNER_AND_ITER_LO:
988 lo = InnerIterativeLocalOptimization::create(estimator, quality, lo_sampler,
989 points_size, threshold, true, params->getLOIterativeSampleSize(),
990 params->getLOInnerMaxIters(), params->getLOIterativeMaxIters(),
991 params->getLOThresholdMultiplier()); break;
992 case LocalOptimMethod::LOCAL_OPTIM_GC:
993 lo = GraphCut::create(estimator, error, quality, graph, lo_sampler, threshold,
994 params->getGraphCutSpatialCoherenceTerm(), params->getLOInnerMaxIters()); break;
995 case LocalOptimMethod::LOCAL_OPTIM_SIGMA:
996 lo = SigmaConsensus::create(estimator, error, quality, verifier,
997 params->getLOSampleSize(), params->getLOInnerMaxIters(),
998 params->getDegreesOfFreedom(), params->getSigmaQuantile(),
999 params->getUpperIncompleteOfSigmaQuantile(), params->getC(), max_thr); break;
1000 default: CV_Error(cv::Error::StsNotImplemented , "Local Optimization is not implemented!");
1001 }
1002 }
1003
1004 if (params->getFinalPolisher() == PolishingMethod::LSQPolisher)
1005 polisher = LeastSquaresPolishing::create(estimator, quality, params->getFinalLSQIterations());
1006
1007 Ransac ransac (params, points_size, estimator, quality, sampler,
1008 termination, verifier, degeneracy, lo, polisher, params->isParallel(), state);
1009 if (ransac.run(ransac_output)) {
1010 if (params->isPnP()) {
1011 // convert R to rodrigues and back and recalculate inliers which due to numerical
1012 // issues can differ
1013 Mat out, R, newR, newP, t, rvec;
1014 if (K1.empty()) {
1015 usac::Utils::decomposeProjection (ransac_output->getModel(), K1, R, t);
1016 Rodrigues(R, rvec);
1017 hconcat(rvec, t, out);
1018 hconcat(out, K1, out);
1019 } else {
1020 const Mat Rt = K1.inv() * ransac_output->getModel();
1021 t = Rt.col(3);
1022 Rodrigues(Rt.colRange(0,3), rvec);
1023 hconcat(rvec, t, out);
1024 }
1025 Rodrigues(rvec, newR);
1026 hconcat(K1 * newR, K1 * t, newP);
1027 std::vector<bool> inliers_mask(points_size);
1028 quality->getInliers(newP, inliers_mask);
1029 ransac_output = RansacOutput::create(out, inliers_mask, 0,0,0,0,0,0);
1030 }
1031 return true;
1032 }
1033 return false;
1034 }
1035 }}
1036