modules/calib3d/src/usac/ransac_solvers.cpp

   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> &params, 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:
  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      */
  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.
  65     const std::vector<bool> &getInliersMask() const override { return inliers_mask; }
  66
  67     int getTimeMicroSeconds() const override {return time_mcs; }
  68     int getTimeMicroSeconds1() const override {return microseconds; }
  69     int getTimeMilliSeconds2() const override {return milliseconds; }
  70     int getTimeSeconds3() const override {return seconds; }
  71     int getNumberOfInliers() const override { return number_inliers; }
  72     int getNumberOfMainIterations() const override { return number_iterations; }
  73     int getNumberOfGoodModels () const override { return number_good_models; }
  74     int getNumberOfEstimatedModels () const override { return number_estimated_models; }
  75     const Mat &getModel() const override { return model; }
  76 };
  77
  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
 101     Ransac (const Ptr<const Model> &params_, 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> &degeneracy_,
 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
 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  */
 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
 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 }
 418 void setParameters (Ptr<Model> &params, 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
 431 void setParameters (int flag, Ptr<Model> &params, 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
 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
 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
 522 Mat findEssentialMat (InputArray points1, InputArray points2, InputArray cameraMatrix1,
 523         int method, double prob, double thr, OutputArray mask, int maxIters) {
 524     Ptr<Model> params;
 525     setParameters(method, params, EstimationMethod::Essential, thr, maxIters, 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
 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
 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:
 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     }
 684     void setVerifier (VerificationMethod verifier_) override { verifier = verifier_; }
 685     void setPolisher (PolishingMethod polisher_) override { polisher = polisher_; }
 686     void setParallel (bool is_parallel_) override { is_parallel = is_parallel_; }
 687     void setError (ErrorMetric error_) override { est_error = error_; }
 688     void setLocalOptimization (LocalOptimMethod lo_) override { lo = lo_; }
 689     void setKNearestNeighhbors (int knn_) override { k_nearest_neighbors = knn_; }
 690     void setNeighborsType (NeighborSearchMethod neighbors) override { neighborsType = neighbors; }
 691     void setCellSize (int cell_size_) override { cell_size = cell_size_; }
 692     void setLOIterations (int iters) override { lo_inner_iterations = iters; }
 693     void setLOIterativeIters (int iters) override {lo_iterative_iterations = iters; }
 694     void setLOSampleSize (int lo_sample_size_) override { lo_sample_size = lo_sample_size_; }
 695     void setThresholdMultiplierLO (double thr_mult) override { lo_thr_multiplier = (int) round(thr_mult); }
 696     void maskRequired (bool need_mask_) override { need_mask = need_mask_; }
 697     void setRandomGeneratorState (int state) override { random_generator_state = state; }
 698     bool isMaskRequired () const override { return need_mask; }
 699     NeighborSearchMethod getNeighborsSearch () const override { return neighborsType; }
 700     int getKNN () const override { return k_nearest_neighbors; }
 701     ErrorMetric getError () const override { return est_error; }
 702     EstimationMethod getEstimator () const override { return estimator; }
 703     int getSampleSize () const override { return sample_size; }
 704     int getFinalLSQIterations () const override { return final_lsq_iters; }
 705     int getDegreesOfFreedom () const override { return DoF; }
 706     double getSigmaQuantile () const override { return sigma_quantile; }
 707     double getUpperIncompleteOfSigmaQuantile () const override {
 708         return upper_incomplete_of_sigma_quantile;
 709     }
 710     double getLowerIncompleteOfSigmaQuantile () const override {
 711         return lower_incomplete_of_sigma_quantile;
 712     }
 713     double getC () const override { return C; }
 714     double getMaximumThreshold () const override { return maximum_thr; }
 715     double getGraphCutSpatialCoherenceTerm () const override { return spatial_coherence_term; }
 716     int getLOSampleSize () const override { return lo_sample_size; }
 717     int getMaxNumHypothesisToTestBeforeRejection() const override {
 718         return max_hypothesis_test_before_verification;
 719     }
 720     PolishingMethod getFinalPolisher () const override { return polisher; }
 721     int getLOThresholdMultiplier() const override { return lo_thr_multiplier; }
 722     int getLOIterativeSampleSize() const override { return lo_iter_sample_size; }
 723     int getLOIterativeMaxIters() const override { return lo_iterative_iterations; }
 724     int getLOInnerMaxIters() const override { return lo_inner_iterations; }
 725     LocalOptimMethod getLO () const override { return lo; }
 726     ScoreMethod getScore () const override { return score; }
 727     int getMaxIters () const override { return max_iterations; }
 728     double getConfidence () const override { return confidence; }
 729     double getThreshold () const override { return threshold; }
 730     VerificationMethod getVerifier () const override { return verifier; }
 731     SamplingMethod getSampler () const override { return sampler; }
 732     int getRandomGeneratorState () const override { return random_generator_state; }
 733     int getMaxItersBeforeLO () const override { return max_iters_before_LO; }
 734     double getSPRTdelta () const override { return sprt_delta; }
 735     double getSPRTepsilon () const override { return sprt_eps; }
 736     double getSPRTavgNumModels () const override { return avg_num_models; }
 737     int getCellSize () const override { return cell_size; }
 738     int getGraphRadius() const override { return radius; }
 739     double getTimeForModelEstimation () const override { return time_for_model_est; }
 740     double getRelaxCoef () const override { return relax_coef; }
 741     const std::vector<int> &getGridCellNumber () const override { return grid_cell_number; }
 742     bool isParallel () const override { return is_parallel; }
 743     bool isFundamental () const override {
 744         return estimator == EstimationMethod ::Fundamental ||
 745                estimator == EstimationMethod ::Fundamental8;
 746     }
 747     bool isHomography () const override { return estimator == EstimationMethod ::Homography; }
 748     bool isEssential () const override { return estimator == EstimationMethod ::Essential; }
 749     bool isPnP() const override {
 750         return estimator == EstimationMethod ::P3P || estimator == EstimationMethod ::P6P;
 751     }
 752 };
 753
 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
 760 bool run (const Ptr<const Model> &params, 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, calib_points, undist_points1, undist_points2;
 778     Matx33d K1, K2;
 779     int points_size;
 780     double threshold = params->getThreshold(), max_thr = params->getMaximumThreshold();
 781     const int min_sample_size = params->getSampleSize();
 782     if (params->isPnP()) {
 783         if (! K1_.empty()) {
 784             K1 = K1_.getMat();
 785             if (! dist_coeff1.empty()) {
 786                 // undistortPoints also calibrate points using K
 787                 if (points1.isContinuous())
 788                      undistortPoints(points1, undist_points1, K1_, dist_coeff1);
 789                 else undistortPoints(points1.getMat().clone(), undist_points1, K1_, dist_coeff1);
 790                 points_size = mergePoints(undist_points1, points2, points, true);
 791                 Utils::normalizeAndDecalibPointsPnP (K1, points, calib_points);
 792             } else {
 793                 points_size = mergePoints(points1, points2, points, true);
 794                 Utils::calibrateAndNormalizePointsPnP(K1, points, calib_points);
 795             }
 796         } else
 797             points_size = mergePoints(points1, points2, points, true);
 798     } else {
 799         if (params->isEssential()) {
 800             CV_CheckEQ((int)(!K1_.empty() && !K2_.empty()), 1, "Intrinsic matrix must not be empty!");
 801             K1 = K1_.getMat();
 802             K2 = K2_.getMat();
 803             if (! dist_coeff1.empty() || ! dist_coeff2.empty()) {
 804                 // undistortPoints also calibrate points using K
 805                 if (points1.isContinuous())
 806                      undistortPoints(points1, undist_points1, K1_, dist_coeff1);
 807                 else undistortPoints(points1.getMat().clone(), undist_points1, K1_, dist_coeff1);
 808                 if (points2.isContinuous())
 809                      undistortPoints(points2, undist_points2, K2_, dist_coeff2);
 810                 else undistortPoints(points2.getMat().clone(), undist_points2, K2_, dist_coeff2);
 811                 points_size = mergePoints(undist_points1, undist_points2, calib_points, false);
 812             } else {
 813                 points_size = mergePoints(points1, points2, points, false);
 814                 Utils::calibratePoints(K1, K2, points, calib_points);
 815             }
 816             threshold = Utils::getCalibratedThreshold(threshold, K1, K2);
 817             max_thr = Utils::getCalibratedThreshold(max_thr, K1, K2);
 818         } else
 819             points_size = mergePoints(points1, points2, points, false);
 820     }
 821
 822     // Since error function output squared error distance, so make
 823     // threshold squared as well
 824     threshold *= threshold;
 825
 826     if (params->getSampler() == SamplingMethod::SAMPLING_NAPSAC || params->getLO() == LocalOptimMethod::LOCAL_OPTIM_GC) {
 827         if (params->getNeighborsSearch() == NeighborSearchMethod::NEIGH_GRID) {
 828             graph = GridNeighborhoodGraph::create(points, points_size,
 829                 params->getCellSize(), params->getCellSize(),
 830                 params->getCellSize(), params->getCellSize(), 10);
 831         } else if (params->getNeighborsSearch() == NeighborSearchMethod::NEIGH_FLANN_KNN) {
 832             graph = FlannNeighborhoodGraph::create(points, points_size,params->getKNN(), false, 5, 1);
 833         } else if (params->getNeighborsSearch() == NeighborSearchMethod::NEIGH_FLANN_RADIUS) {
 834             graph = RadiusSearchNeighborhoodGraph::create(points, points_size,
 835                     params->getGraphRadius(), 5, 1);
 836         } else CV_Error(cv::Error::StsNotImplemented, "Graph type is not implemented!");
 837     }
 838
 839     std::vector<Ptr<NeighborhoodGraph>> layers;
 840     if (params->getSampler() == SamplingMethod::SAMPLING_PROGRESSIVE_NAPSAC) {
 841         CV_CheckEQ((int)params->isPnP(), 0, "ProgressiveNAPSAC for PnP is not implemented!");
 842         const auto &cell_number_per_layer = params->getGridCellNumber();
 843         layers.reserve(cell_number_per_layer.size());
 844         const auto * const pts = (float *) points.data;
 845         float img1_width = 0, img1_height = 0, img2_width = 0, img2_height = 0;
 846         for (int i = 0; i < 4 * points_size; i += 4) {
 847             if (pts[i    ] > img1_width ) img1_width  = pts[i    ];
 848             if (pts[i + 1] > img1_height) img1_height = pts[i + 1];
 849             if (pts[i + 2] > img2_width ) img2_width  = pts[i + 2];
 850             if (pts[i + 3] > img2_height) img2_height = pts[i + 3];
 851         }
 852         // Create grid graphs (overlapping layes of given cell numbers)
 853         for (int layer_idx = 0; layer_idx < (int)cell_number_per_layer.size(); layer_idx++) {
 854             const int cell_number = cell_number_per_layer[layer_idx];
 855             if (layer_idx > 0)
 856                 if (cell_number_per_layer[layer_idx-1] <= cell_number)
 857                     CV_Error(cv::Error::StsError, "Progressive NAPSAC sampler: "
 858                         "Cell number in layers must be in decreasing order!");
 859             layers.emplace_back(GridNeighborhoodGraph::create(points, points_size,
 860           (int)(img1_width / (float)cell_number), (int)(img1_height / (float)cell_number),
 861           (int)(img2_width / (float)cell_number), (int)(img2_height / (float)cell_number), 10));
 862         }
 863     }
 864
 865     // update points by calibrated for Essential matrix after graph is calculated
 866     if (params->isEssential()) {
 867         points = calib_points;
 868         // if maximum calibrated threshold significanlty differs threshold then set upper bound
 869         if (max_thr > 10*threshold)
 870             max_thr = sqrt(10*threshold); // max thr will be squared after
 871     }
 872     if (max_thr < threshold)
 873         max_thr = threshold;
 874
 875     switch (params->getError()) {
 876         case ErrorMetric::SYMM_REPR_ERR:
 877             error = ReprojectionErrorSymmetric::create(points); break;
 878         case ErrorMetric::FORW_REPR_ERR:
 879             if (params->getEstimator() == EstimationMethod::Affine)
 880                 error = ReprojectionErrorAffine::create(points);
 881             else error = ReprojectionErrorForward::create(points);
 882             break;
 883         case ErrorMetric::SAMPSON_ERR:
 884             error = SampsonError::create(points); break;
 885         case ErrorMetric::SGD_ERR:
 886             error = SymmetricGeometricDistance::create(points); break;
 887         case ErrorMetric::RERPOJ:
 888             error = ReprojectionErrorPmatrix::create(points); break;
 889         default: CV_Error(cv::Error::StsNotImplemented , "Error metric is not implemented!");
 890     }
 891
 892     switch (params->getScore()) {
 893         case ScoreMethod::SCORE_METHOD_RANSAC :
 894             quality = RansacQuality::create(points_size, threshold, error); break;
 895         case ScoreMethod::SCORE_METHOD_MSAC :
 896             quality = MsacQuality::create(points_size, threshold, error); break;
 897         case ScoreMethod::SCORE_METHOD_MAGSAC :
 898             quality = MagsacQuality::create(max_thr, points_size, error,
 899                 threshold, params->getDegreesOfFreedom(),  params->getSigmaQuantile(),
 900                 params->getUpperIncompleteOfSigmaQuantile(),
 901                 params->getLowerIncompleteOfSigmaQuantile(), params->getC()); break;
 902         case ScoreMethod::SCORE_METHOD_LMEDS :
 903             quality = LMedsQuality::create(points_size, threshold, error); break;
 904         default: CV_Error(cv::Error::StsNotImplemented, "Score is not imeplemeted!");
 905     }
 906
 907     if (params->isHomography()) {
 908         degeneracy = HomographyDegeneracy::create(points);
 909         min_solver = HomographyMinimalSolver4ptsGEM::create(points);
 910         non_min_solver = HomographyNonMinimalSolver::create(points);
 911         estimator = HomographyEstimator::create(min_solver, non_min_solver, degeneracy);
 912     } else if (params->isFundamental()) {
 913         degeneracy = FundamentalDegeneracy::create(state++, quality, points, min_sample_size, 5. /*sqr homogr thr*/);
 914         if(min_sample_size == 7) min_solver = FundamentalMinimalSolver7pts::create(points);
 915         else min_solver = FundamentalMinimalSolver8pts::create(points);
 916         non_min_solver = FundamentalNonMinimalSolver::create(points);
 917         estimator = FundamentalEstimator::create(min_solver, non_min_solver, degeneracy);
 918     } else if (params->isEssential()) {
 919         degeneracy = EssentialDegeneracy::create(points, min_sample_size);
 920         min_solver = EssentialMinimalSolverStewenius5pts::create(points);
 921         non_min_solver = EssentialNonMinimalSolver::create(points);
 922         estimator = EssentialEstimator::create(min_solver, non_min_solver, degeneracy);
 923     } else if (params->isPnP()) {
 924         degeneracy = makePtr<Degeneracy>();
 925         if (min_sample_size == 3) {
 926             non_min_solver = DLSPnP::create(points, calib_points, K1);
 927             min_solver = P3PSolver::create(points, calib_points, K1);
 928         } else {
 929             min_solver = PnPMinimalSolver6Pts::create(points);
 930             non_min_solver = PnPNonMinimalSolver::create(points);
 931         }
 932         estimator = PnPEstimator::create(min_solver, non_min_solver);
 933     } else if (params->getEstimator() == EstimationMethod::Affine) {
 934         degeneracy = makePtr<Degeneracy>();
 935         min_solver = AffineMinimalSolver::create(points);
 936         non_min_solver = AffineNonMinimalSolver::create(points);
 937         estimator = AffineEstimator::create(min_solver, non_min_solver);
 938     } else CV_Error(cv::Error::StsNotImplemented, "Estimator not implemented!");
 939
 940     switch (params->getSampler()) {
 941         case SamplingMethod::SAMPLING_UNIFORM:
 942             sampler = UniformSampler::create(state++, min_sample_size, points_size); break;
 943         case SamplingMethod::SAMPLING_PROSAC:
 944             sampler = ProsacSampler::create(state++, points_size, min_sample_size, 200000); break;
 945         case SamplingMethod::SAMPLING_PROGRESSIVE_NAPSAC:
 946             sampler = ProgressiveNapsac::create(state++, points_size, min_sample_size, layers, 20); break;
 947         case SamplingMethod::SAMPLING_NAPSAC:
 948             sampler = NapsacSampler::create(state++, points_size, min_sample_size, graph); break;
 949         default: CV_Error(cv::Error::StsNotImplemented, "Sampler is not implemented!");
 950     }
 951
 952     switch (params->getVerifier()) {
 953         case VerificationMethod::NullVerifier: verifier = ModelVerifier::create(); break;
 954         case VerificationMethod::SprtVerifier:
 955             verifier = SPRT::create(state++, error, points_size, params->getScore() == ScoreMethod ::SCORE_METHOD_MAGSAC ? max_thr : threshold,
 956              params->getSPRTepsilon(), params->getSPRTdelta(), params->getTimeForModelEstimation(),
 957              params->getSPRTavgNumModels(), params->getScore()); break;
 958         default: CV_Error(cv::Error::StsNotImplemented, "Verifier is not imeplemented!");
 959     }
 960
 961     if (params->getSampler() == SamplingMethod::SAMPLING_PROSAC) {
 962         termination = ProsacTerminationCriteria::create(sampler.dynamicCast<ProsacSampler>(), error,
 963                 points_size, min_sample_size, params->getConfidence(),
 964                 params->getMaxIters(), 100, 0.05, 0.05, threshold);
 965     } else if (params->getSampler() == SamplingMethod::SAMPLING_PROGRESSIVE_NAPSAC) {
 966         if (params->getVerifier() == VerificationMethod::SprtVerifier)
 967             termination = SPRTPNapsacTermination::create(((SPRT *)verifier.get())->getSPRTvector(),
 968                     params->getConfidence(), points_size, min_sample_size,
 969                     params->getMaxIters(), params->getRelaxCoef());
 970         else
 971             termination = StandardTerminationCriteria::create (params->getConfidence(),
 972                     points_size, min_sample_size, params->getMaxIters());
 973     } else if (params->getVerifier() == VerificationMethod::SprtVerifier) {
 974         termination = SPRTTermination::create(((SPRT *) verifier.get())->getSPRTvector(),
 975              params->getConfidence(), points_size, min_sample_size, params->getMaxIters());
 976     } else
 977         termination = StandardTerminationCriteria::create
 978             (params->getConfidence(), points_size, min_sample_size, params->getMaxIters());
 979
 980     if (params->getLO() != LocalOptimMethod::LOCAL_OPTIM_NULL) {
 981         lo_sampler = UniformRandomGenerator::create(state++, points_size, params->getLOSampleSize());
 982         switch (params->getLO()) {
 983             case LocalOptimMethod::LOCAL_OPTIM_INNER_LO:
 984                 lo = InnerIterativeLocalOptimization::create(estimator, quality, lo_sampler,
 985                      points_size, threshold, false, params->getLOIterativeSampleSize(),
 986                      params->getLOInnerMaxIters(), params->getLOIterativeMaxIters(),
 987                      params->getLOThresholdMultiplier()); break;
 988             case LocalOptimMethod::LOCAL_OPTIM_INNER_AND_ITER_LO:
 989                 lo = InnerIterativeLocalOptimization::create(estimator, quality, lo_sampler,
 990                      points_size, threshold, true, params->getLOIterativeSampleSize(),
 991                      params->getLOInnerMaxIters(), params->getLOIterativeMaxIters(),
 992                      params->getLOThresholdMultiplier()); break;
 993             case LocalOptimMethod::LOCAL_OPTIM_GC:
 994                 lo = GraphCut::create(estimator, error, quality, graph, lo_sampler, threshold,
 995                    params->getGraphCutSpatialCoherenceTerm(), params->getLOInnerMaxIters()); break;
 996             case LocalOptimMethod::LOCAL_OPTIM_SIGMA:
 997                 lo = SigmaConsensus::create(estimator, error, quality, verifier,
 998                      params->getLOSampleSize(), params->getLOInnerMaxIters(),
 999                      params->getDegreesOfFreedom(), params->getSigmaQuantile(),
1000                      params->getUpperIncompleteOfSigmaQuantile(), params->getC(), max_thr); break;
1001             default: CV_Error(cv::Error::StsNotImplemented , "Local Optimization is not implemented!");
1002         }
1003     }
1004
1005     if (params->getFinalPolisher() == PolishingMethod::LSQPolisher)
1006         polisher = LeastSquaresPolishing::create(estimator, quality, params->getFinalLSQIterations());
1007
1008     Ransac ransac (params, points_size, estimator, quality, sampler,
1009           termination, verifier, degeneracy, lo, polisher, params->isParallel(), state);
1010     if (ransac.run(ransac_output)) {
1011         if (params->isPnP()) {
1012             // convert R to rodrigues and back and recalculate inliers which due to numerical
1013             // issues can differ
1014             Mat out, R, newR, newP, t, rvec;
1015             if (K1_.empty()) {
1016                 usac::Utils::decomposeProjection (ransac_output->getModel(), K1, R, t);
1017                 Rodrigues(R, rvec);
1018                 hconcat(rvec, t, out);
1019                 hconcat(out, K1, out);
1020             } else {
1021                 const Mat Rt = K1.inv() * ransac_output->getModel();
1022                 t = Rt.col(3);
1023                 Rodrigues(Rt.colRange(0,3), rvec);
1024                 hconcat(rvec, t, out);
1025             }
1026             Rodrigues(rvec, newR);
1027             hconcat(K1 * newR, K1 * t, newP);
1028             std::vector<bool> inliers_mask(points_size);
1029             quality->getInliers(newP, inliers_mask);
1030             ransac_output = RansacOutput::create(out, inliers_mask, 0,0,0,0,0,0);
1031         }
1032         return true;
1033     }
1034     return false;
1035 }
1036 }}