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->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         mask.create(points_size, 1, CV_8U);
 412         auto * maskptr = mask.getMat().ptr<uchar>();
 413         for (int i = 0; i < points_size; i++)
 414             maskptr[i] = (uchar) inliers_mask[i];
 415     }
 416 }
 417 void setParameters (Ptr<Model> &params, EstimationMethod estimator, const UsacParams &usac_params,
 418         bool mask_needed) {
 419     params = Model::create(usac_params.threshold, estimator, usac_params.sampler,
 420             usac_params.confidence, usac_params.maxIterations, usac_params.score);
 421     params->setLocalOptimization(usac_params.loMethod);
 422     params->setLOSampleSize(usac_params.loSampleSize);
 423     params->setLOIterations(usac_params.loIterations);
 424     params->setParallel(usac_params.isParallel);
 425     params->setNeighborsType(usac_params.neighborsSearch);
 426     params->setRandomGeneratorState(usac_params.randomGeneratorState);
 427     params->maskRequired(mask_needed);
 428 }
 429
 430 void setParameters (int flag, Ptr<Model> &params, EstimationMethod estimator, double thr,
 431         int max_iters, double conf, bool mask_needed) {
 432     switch (flag) {
 433         case USAC_DEFAULT:
 434             params = Model::create(thr, estimator, SamplingMethod::SAMPLING_UNIFORM, conf, max_iters,
 435                                    ScoreMethod::SCORE_METHOD_MSAC);
 436             params->setLocalOptimization(LocalOptimMethod ::LOCAL_OPTIM_INNER_AND_ITER_LO);
 437             break;
 438         case USAC_MAGSAC:
 439             params = Model::create(thr, estimator, SamplingMethod::SAMPLING_UNIFORM, conf, max_iters,
 440                                    ScoreMethod::SCORE_METHOD_MAGSAC);
 441             params->setLocalOptimization(LocalOptimMethod ::LOCAL_OPTIM_SIGMA);
 442             params->setLOSampleSize(params->isHomography() ? 75 : 50);
 443             params->setLOIterations(params->isHomography() ? 15 : 10);
 444             break;
 445         case USAC_PARALLEL:
 446             params = Model::create(thr, estimator, SamplingMethod::SAMPLING_UNIFORM, conf, max_iters,
 447                                    ScoreMethod::SCORE_METHOD_MSAC);
 448             params->setParallel(true);
 449             params->setLocalOptimization(LocalOptimMethod ::LOCAL_OPTIM_INNER_LO);
 450             break;
 451         case USAC_ACCURATE:
 452             params = Model::create(thr, estimator, SamplingMethod::SAMPLING_UNIFORM, conf, max_iters,
 453                                    ScoreMethod::SCORE_METHOD_MSAC);
 454             params->setLocalOptimization(LocalOptimMethod ::LOCAL_OPTIM_GC);
 455             params->setLOSampleSize(20);
 456             params->setLOIterations(25);
 457             break;
 458         case USAC_FAST:
 459             params = Model::create(thr, estimator, SamplingMethod::SAMPLING_UNIFORM, conf, max_iters,
 460                                    ScoreMethod::SCORE_METHOD_MSAC);
 461             params->setLocalOptimization(LocalOptimMethod ::LOCAL_OPTIM_INNER_AND_ITER_LO);
 462             params->setLOIterations(5);
 463             params->setLOIterativeIters(3);
 464             break;
 465         case USAC_PROSAC:
 466             params = Model::create(thr, estimator, SamplingMethod::SAMPLING_PROSAC, conf, max_iters,
 467                                    ScoreMethod::SCORE_METHOD_MSAC);
 468             params->setLocalOptimization(LocalOptimMethod ::LOCAL_OPTIM_INNER_LO);
 469             break;
 470         case USAC_FM_8PTS:
 471             params = Model::create(thr, EstimationMethod::Fundamental8,SamplingMethod::SAMPLING_UNIFORM,
 472                     conf, max_iters,ScoreMethod::SCORE_METHOD_MSAC);
 473             params->setLocalOptimization(LocalOptimMethod ::LOCAL_OPTIM_INNER_LO);
 474             break;
 475         default: CV_Error(cv::Error::StsBadFlag, "Incorrect flag for USAC!");
 476     }
 477     // do not do too many iterations for PnP
 478     if (estimator == EstimationMethod::P3P) {
 479         if (params->getLOInnerMaxIters() > 15)
 480             params->setLOIterations(15);
 481         params->setLOIterativeIters(0);
 482     }
 483
 484     params->maskRequired(mask_needed);
 485 }
 486
 487 Mat findHomography (InputArray srcPoints, InputArray dstPoints, int method, double thr,
 488         OutputArray mask, const int max_iters, const double confidence) {
 489     Ptr<Model> params;
 490     setParameters(method, params, EstimationMethod::Homography, thr, max_iters, confidence, mask.needed());
 491     Ptr<RansacOutput> ransac_output;
 492     if (run(params, srcPoints, dstPoints, params->getRandomGeneratorState(),
 493             ransac_output, noArray(), noArray(), noArray(), noArray())) {
 494         saveMask(mask, ransac_output->getInliersMask());
 495         return ransac_output->getModel() / ransac_output->getModel().at<double>(2,2);
 496     }
 497     if (mask.needed()){
 498         mask.create(std::max(srcPoints.getMat().rows, srcPoints.getMat().cols), 1, CV_8U);
 499         mask.setTo(Scalar::all(0));
 500     }
 501     return Mat();
 502 }
 503
 504 Mat findFundamentalMat( InputArray points1, InputArray points2, int method, double thr,
 505         double confidence, int max_iters, OutputArray mask ) {
 506     Ptr<Model> params;
 507     setParameters(method, params, EstimationMethod::Fundamental, thr, max_iters, confidence, mask.needed());
 508     Ptr<RansacOutput> ransac_output;
 509     if (run(params, points1, points2, params->getRandomGeneratorState(),
 510             ransac_output, noArray(), noArray(), noArray(), noArray())) {
 511         saveMask(mask, ransac_output->getInliersMask());
 512         return ransac_output->getModel();
 513     }
 514     if (mask.needed()){
 515         mask.create(std::max(points1.getMat().rows, points1.getMat().cols), 1, CV_8U);
 516         mask.setTo(Scalar::all(0));
 517     }
 518     return Mat();
 519 }
 520
 521 Mat findEssentialMat (InputArray points1, InputArray points2, InputArray cameraMatrix1,
 522         int method, double prob, double thr, OutputArray mask) {
 523     Ptr<Model> params;
 524     setParameters(method, params, EstimationMethod::Essential, thr, 1000, prob, mask.needed());
 525     Ptr<RansacOutput> ransac_output;
 526     if (run(params, points1, points2, params->getRandomGeneratorState(),
 527             ransac_output, cameraMatrix1, cameraMatrix1, noArray(), noArray())) {
 528         saveMask(mask, ransac_output->getInliersMask());
 529         return ransac_output->getModel();
 530     }
 531     if (mask.needed()){
 532         mask.create(std::max(points1.getMat().rows, points1.getMat().cols), 1, CV_8U);
 533         mask.setTo(Scalar::all(0));
 534     }
 535     return Mat();
 536 }
 537
 538 bool solvePnPRansac( InputArray objectPoints, InputArray imagePoints,
 539        InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec,
 540        bool /*useExtrinsicGuess*/, int max_iters, float thr, double conf,
 541        OutputArray mask, int method) {
 542     Ptr<Model> params;
 543     setParameters(method, params, cameraMatrix.empty() ? EstimationMethod ::P6P : EstimationMethod ::P3P,
 544             thr, max_iters, conf, mask.needed());
 545     Ptr<RansacOutput> ransac_output;
 546     if (run(params, imagePoints, objectPoints, params->getRandomGeneratorState(),
 547             ransac_output, cameraMatrix, noArray(), distCoeffs, noArray())) {
 548         saveMask(mask, ransac_output->getInliersMask());
 549         const Mat &model = ransac_output->getModel();
 550         model.col(0).copyTo(rvec);
 551         model.col(1).copyTo(tvec);
 552         return true;
 553     }
 554     if (mask.needed()){
 555         mask.create(std::max(objectPoints.getMat().rows, objectPoints.getMat().cols), 1, CV_8U);
 556         mask.setTo(Scalar::all(0));
 557     }
 558     return false;
 559 }
 560
 561 Mat estimateAffine2D(InputArray from, InputArray to, OutputArray mask, int method,
 562         double thr, int max_iters, double conf, int /*refineIters*/) {
 563     Ptr<Model> params;
 564     setParameters(method, params, EstimationMethod ::Affine, thr, max_iters, conf, mask.needed());
 565     Ptr<RansacOutput> ransac_output;
 566     if (run(params, from, to, params->getRandomGeneratorState(),
 567             ransac_output, noArray(), noArray(), noArray(), noArray())) {
 568         saveMask(mask, ransac_output->getInliersMask());
 569         return ransac_output->getModel().rowRange(0,2);
 570     }
 571     if (mask.needed()){
 572         mask.create(std::max(from.getMat().rows, from.getMat().cols), 1, CV_8U);
 573         mask.setTo(Scalar::all(0));
 574     }
 575     return Mat();
 576 }
 577
 578 class ModelImpl : public Model {
 579 private:
 580     // main parameters:
 581     double threshold, confidence;
 582     int sample_size, max_iterations;
 583
 584     EstimationMethod estimator;
 585     SamplingMethod sampler;
 586     ScoreMethod score;
 587
 588     // for neighborhood graph
 589     int k_nearest_neighbors = 8;//, flann_search_params = 5, num_kd_trees = 1; // for FLANN
 590     int cell_size = 50; // pixels, for grid neighbors searching
 591     int radius = 30; // pixels, for radius-search neighborhood graph
 592     NeighborSearchMethod neighborsType = NeighborSearchMethod::NEIGH_GRID;
 593
 594     // Local Optimization parameters
 595     LocalOptimMethod lo = LocalOptimMethod ::LOCAL_OPTIM_INNER_AND_ITER_LO;
 596     int lo_sample_size=16, lo_inner_iterations=15, lo_iterative_iterations=8,
 597             lo_thr_multiplier=15, lo_iter_sample_size = 30;
 598
 599     // Graph cut parameters
 600     const double spatial_coherence_term = 0.975;
 601
 602     // apply polisher for final RANSAC model
 603     PolishingMethod polisher = PolishingMethod ::LSQPolisher;
 604
 605     // preemptive verification test
 606     VerificationMethod verifier = VerificationMethod ::SprtVerifier;
 607     const int max_hypothesis_test_before_verification = 15;
 608
 609     // sprt parameters
 610     // lower bound estimate is 1% of inliers
 611     double sprt_eps = 0.01, sprt_delta = 0.008, avg_num_models, time_for_model_est;
 612
 613     // estimator error
 614     ErrorMetric est_error;
 615
 616     // progressive napsac
 617     double relax_coef = 0.1;
 618     // for building neighborhood graphs
 619     const std::vector<int> grid_cell_number = {16, 8, 4, 2};
 620
 621     //for final least squares polisher
 622     int final_lsq_iters = 3;
 623
 624     bool need_mask = true, is_parallel = false;
 625     int random_generator_state = 0;
 626     const int max_iters_before_LO = 100;
 627
 628     // magsac parameters:
 629     int DoF = 2;
 630     double sigma_quantile = 3.04, upper_incomplete_of_sigma_quantile = 0.00419,
 631         lower_incomplete_of_sigma_quantile = 0.8629, C = 0.5, maximum_thr = 7.5;
 632 public:
 633     ModelImpl (double threshold_, EstimationMethod estimator_, SamplingMethod sampler_, double confidence_=0.95,
 634                int max_iterations_=5000, ScoreMethod score_ =ScoreMethod::SCORE_METHOD_MSAC) {
 635         estimator = estimator_;
 636         sampler = sampler_;
 637         confidence = confidence_;
 638         max_iterations = max_iterations_;
 639         score = score_;
 640
 641         switch (estimator_) {
 642             // time for model estimation is basically a ratio of time need to estimate a model to
 643             // time needed to verify if a point is consistent with this model
 644             case (EstimationMethod::Affine):
 645                 avg_num_models = 1; time_for_model_est = 50;
 646                 sample_size = 3; est_error = ErrorMetric ::FORW_REPR_ERR; break;
 647             case (EstimationMethod::Homography):
 648                 avg_num_models = 1; time_for_model_est = 150;
 649                 sample_size = 4; est_error = ErrorMetric ::FORW_REPR_ERR; break;
 650             case (EstimationMethod::Fundamental):
 651                 avg_num_models = 2.38; time_for_model_est = 180; maximum_thr = 2.5;
 652                 sample_size = 7; est_error = ErrorMetric ::SAMPSON_ERR; break;
 653             case (EstimationMethod::Fundamental8):
 654                 avg_num_models = 1; time_for_model_est = 100; maximum_thr = 2.5;
 655                 sample_size = 8; est_error = ErrorMetric ::SAMPSON_ERR; break;
 656             case (EstimationMethod::Essential):
 657                 avg_num_models = 3.93; time_for_model_est = 1000; maximum_thr = 2.5;
 658                 sample_size = 5; est_error = ErrorMetric ::SGD_ERR; break;
 659             case (EstimationMethod::P3P):
 660                 avg_num_models = 1.38; time_for_model_est = 800;
 661                 sample_size = 3; est_error = ErrorMetric ::RERPOJ; break;
 662             case (EstimationMethod::P6P):
 663                 avg_num_models = 1; time_for_model_est = 300;
 664                 sample_size = 6; est_error = ErrorMetric ::RERPOJ; break;
 665             default: CV_Error(cv::Error::StsNotImplemented, "Estimator has not implemented yet!");
 666         }
 667
 668         if (estimator_ == EstimationMethod::P3P || estimator_ == EstimationMethod::P6P) {
 669             neighborsType = NeighborSearchMethod::NEIGH_FLANN_KNN;
 670             k_nearest_neighbors = 2;
 671         }
 672         if (estimator == EstimationMethod::Fundamental || estimator == EstimationMethod::Essential) {
 673             lo_sample_size = 21;
 674             lo_thr_multiplier = 10;
 675         }
 676         if (estimator == EstimationMethod::Homography)
 677             maximum_thr = 8.;
 678         threshold = threshold_;
 679     }
 680     void setVerifier (VerificationMethod verifier_) override { verifier = verifier_; }
 681     void setPolisher (PolishingMethod polisher_) override { polisher = polisher_; }
 682     void setParallel (bool is_parallel_) override { is_parallel = is_parallel_; }
 683     void setError (ErrorMetric error_) override { est_error = error_; }
 684     void setLocalOptimization (LocalOptimMethod lo_) override { lo = lo_; }
 685     void setKNearestNeighhbors (int knn_) override { k_nearest_neighbors = knn_; }
 686     void setNeighborsType (NeighborSearchMethod neighbors) override { neighborsType = neighbors; }
 687     void setCellSize (int cell_size_) override { cell_size = cell_size_; }
 688     void setLOIterations (int iters) override { lo_inner_iterations = iters; }
 689     void setLOIterativeIters (int iters) override {lo_iterative_iterations = iters; }
 690     void setLOSampleSize (int lo_sample_size_) override { lo_sample_size = lo_sample_size_; }
 691     void setThresholdMultiplierLO (double thr_mult) override { lo_thr_multiplier = (int) round(thr_mult); }
 692     void maskRequired (bool need_mask_) override { need_mask = need_mask_; }
 693     void setRandomGeneratorState (int state) override { random_generator_state = state; }
 694     bool isMaskRequired () const override { return need_mask; }
 695     NeighborSearchMethod getNeighborsSearch () const override { return neighborsType; }
 696     int getKNN () const override { return k_nearest_neighbors; }
 697     ErrorMetric getError () const override { return est_error; }
 698     EstimationMethod getEstimator () const override { return estimator; }
 699     int getSampleSize () const override { return sample_size; }
 700     int getFinalLSQIterations () const override { return final_lsq_iters; }
 701     int getDegreesOfFreedom () const override { return DoF; }
 702     double getSigmaQuantile () const override { return sigma_quantile; }
 703     double getUpperIncompleteOfSigmaQuantile () const override {
 704         return upper_incomplete_of_sigma_quantile;
 705     }
 706     double getLowerIncompleteOfSigmaQuantile () const override {
 707         return lower_incomplete_of_sigma_quantile;
 708     }
 709     double getC () const override { return C; }
 710     double getMaximumThreshold () const override { return maximum_thr; }
 711     double getGraphCutSpatialCoherenceTerm () const override { return spatial_coherence_term; }
 712     int getLOSampleSize () const override { return lo_sample_size; }
 713     int getMaxNumHypothesisToTestBeforeRejection() const override {
 714         return max_hypothesis_test_before_verification;
 715     }
 716     PolishingMethod getFinalPolisher () const override { return polisher; }
 717     int getLOThresholdMultiplier() const override { return lo_thr_multiplier; }
 718     int getLOIterativeSampleSize() const override { return lo_iter_sample_size; }
 719     int getLOIterativeMaxIters() const override { return lo_iterative_iterations; }
 720     int getLOInnerMaxIters() const override { return lo_inner_iterations; }
 721     LocalOptimMethod getLO () const override { return lo; }
 722     ScoreMethod getScore () const override { return score; }
 723     int getMaxIters () const override { return max_iterations; }
 724     double getConfidence () const override { return confidence; }
 725     double getThreshold () const override { return threshold; }
 726     VerificationMethod getVerifier () const override { return verifier; }
 727     SamplingMethod getSampler () const override { return sampler; }
 728     int getRandomGeneratorState () const override { return random_generator_state; }
 729     int getMaxItersBeforeLO () const override { return max_iters_before_LO; }
 730     double getSPRTdelta () const override { return sprt_delta; }
 731     double getSPRTepsilon () const override { return sprt_eps; }
 732     double getSPRTavgNumModels () const override { return avg_num_models; }
 733     int getCellSize () const override { return cell_size; }
 734     int getGraphRadius() const override { return radius; }
 735     double getTimeForModelEstimation () const override { return time_for_model_est; }
 736     double getRelaxCoef () const override { return relax_coef; }
 737     const std::vector<int> &getGridCellNumber () const override { return grid_cell_number; }
 738     bool isParallel () const override { return is_parallel; }
 739     bool isFundamental () const override {
 740         return estimator == EstimationMethod ::Fundamental ||
 741                estimator == EstimationMethod ::Fundamental8;
 742     }
 743     bool isHomography () const override { return estimator == EstimationMethod ::Homography; }
 744     bool isEssential () const override { return estimator == EstimationMethod ::Essential; }
 745     bool isPnP() const override {
 746         return estimator == EstimationMethod ::P3P || estimator == EstimationMethod ::P6P;
 747     }
 748 };
 749
 750 Ptr<Model> Model::create(double threshold_, EstimationMethod estimator_, SamplingMethod sampler_,
 751                          double confidence_, int max_iterations_, ScoreMethod score_) {
 752     return makePtr<ModelImpl>(threshold_, estimator_, sampler_, confidence_,
 753                               max_iterations_, score_);
 754 }
 755
 756 bool run (const Ptr<const Model> &params, InputArray points1, InputArray points2, int state,
 757        Ptr<RansacOutput> &ransac_output, InputArray K1_, InputArray K2_,
 758        InputArray dist_coeff1, InputArray dist_coeff2) {
 759     Ptr<Error> error;
 760     Ptr<Estimator> estimator;
 761     Ptr<NeighborhoodGraph> graph;
 762     Ptr<Degeneracy> degeneracy;
 763     Ptr<Quality> quality;
 764     Ptr<ModelVerifier> verifier;
 765     Ptr<Sampler> sampler;
 766     Ptr<RandomGenerator> lo_sampler;
 767     Ptr<TerminationCriteria> termination;
 768     Ptr<LocalOptimization> lo;
 769     Ptr<FinalModelPolisher> polisher;
 770     Ptr<MinimalSolver> min_solver;
 771     Ptr<NonMinimalSolver> non_min_solver;
 772
 773     Mat points, K1, K2, calib_points, undist_points1, undist_points2;
 774     int points_size;
 775     double threshold = params->getThreshold(), max_thr = params->getMaximumThreshold();
 776     const int min_sample_size = params->getSampleSize();
 777     if (params->isPnP()) {
 778         if (! K1_.empty()) {
 779             K1 = K1_.getMat(); K1.convertTo(K1, CV_64F);
 780             if (! dist_coeff1.empty()) {
 781                 // undistortPoints also calibrate points using K
 782                 if (points1.isContinuous())
 783                      undistortPoints(points1, undist_points1, K1_, dist_coeff1);
 784                 else undistortPoints(points1.getMat().clone(), undist_points1, K1_, dist_coeff1);
 785                 points_size = mergePoints(undist_points1, points2, points, true);
 786                 Utils::normalizeAndDecalibPointsPnP (K1, points, calib_points);
 787             } else {
 788                 points_size = mergePoints(points1, points2, points, true);
 789                 Utils::calibrateAndNormalizePointsPnP(K1, points, calib_points);
 790             }
 791         } else
 792             points_size = mergePoints(points1, points2, points, true);
 793     } else {
 794         if (params->isEssential()) {
 795             CV_CheckEQ((int)(!K1_.empty() && !K2_.empty()), 1, "Intrinsic matrix must not be empty!");
 796             K1 = K1_.getMat(); K1.convertTo(K1, CV_64F);
 797             K2 = K2_.getMat(); K2.convertTo(K2, CV_64F);
 798             if (! dist_coeff1.empty() || ! dist_coeff2.empty()) {
 799                 // undistortPoints also calibrate points using K
 800                 if (points1.isContinuous())
 801                      undistortPoints(points1, undist_points1, K1_, dist_coeff1);
 802                 else undistortPoints(points1.getMat().clone(), undist_points1, K1_, dist_coeff1);
 803                 if (points2.isContinuous())
 804                      undistortPoints(points2, undist_points2, K2_, dist_coeff2);
 805                 else undistortPoints(points2.getMat().clone(), undist_points2, K2_, dist_coeff2);
 806                 points_size = mergePoints(undist_points1, undist_points2, calib_points, false);
 807             } else {
 808                 points_size = mergePoints(points1, points2, points, false);
 809                 Utils::calibratePoints(K1, K2, points, calib_points);
 810             }
 811             threshold = Utils::getCalibratedThreshold(threshold, K1, K2);
 812             max_thr = Utils::getCalibratedThreshold(max_thr, K1, K2);
 813         } else
 814             points_size = mergePoints(points1, points2, points, false);
 815     }
 816
 817     // Since error function output squared error distance, so make
 818     // threshold squared as well
 819     threshold *= threshold;
 820
 821     if (params->getSampler() == SamplingMethod::SAMPLING_NAPSAC || params->getLO() == LocalOptimMethod::LOCAL_OPTIM_GC) {
 822         if (params->getNeighborsSearch() == NeighborSearchMethod::NEIGH_GRID) {
 823             graph = GridNeighborhoodGraph::create(points, points_size,
 824                 params->getCellSize(), params->getCellSize(),
 825                 params->getCellSize(), params->getCellSize(), 10);
 826         } else if (params->getNeighborsSearch() == NeighborSearchMethod::NEIGH_FLANN_KNN) {
 827             graph = FlannNeighborhoodGraph::create(points, points_size,params->getKNN(), false, 5, 1);
 828         } else if (params->getNeighborsSearch() == NeighborSearchMethod::NEIGH_FLANN_RADIUS) {
 829             graph = RadiusSearchNeighborhoodGraph::create(points, points_size,
 830                     params->getGraphRadius(), 5, 1);
 831         } else CV_Error(cv::Error::StsNotImplemented, "Graph type is not implemented!");
 832     }
 833
 834     std::vector<Ptr<NeighborhoodGraph>> layers;
 835     if (params->getSampler() == SamplingMethod::SAMPLING_PROGRESSIVE_NAPSAC) {
 836         CV_CheckEQ((int)params->isPnP(), 0, "ProgressiveNAPSAC for PnP is not implemented!");
 837         const auto &cell_number_per_layer = params->getGridCellNumber();
 838         layers.reserve(cell_number_per_layer.size());
 839         const auto * const pts = (float *) points.data;
 840         float img1_width = 0, img1_height = 0, img2_width = 0, img2_height = 0;
 841         for (int i = 0; i < 4 * points_size; i += 4) {
 842             if (pts[i    ] > img1_width ) img1_width  = pts[i    ];
 843             if (pts[i + 1] > img1_height) img1_height = pts[i + 1];
 844             if (pts[i + 2] > img2_width ) img2_width  = pts[i + 2];
 845             if (pts[i + 3] > img2_height) img2_height = pts[i + 3];
 846         }
 847         // Create grid graphs (overlapping layes of given cell numbers)
 848         for (int layer_idx = 0; layer_idx < (int)cell_number_per_layer.size(); layer_idx++) {
 849             const int cell_number = cell_number_per_layer[layer_idx];
 850             if (layer_idx > 0)
 851                 if (cell_number_per_layer[layer_idx-1] <= cell_number)
 852                     CV_Error(cv::Error::StsError, "Progressive NAPSAC sampler: "
 853                         "Cell number in layers must be in decreasing order!");
 854             layers.emplace_back(GridNeighborhoodGraph::create(points, points_size,
 855           (int)(img1_width / (float)cell_number), (int)(img1_height / (float)cell_number),
 856           (int)(img2_width / (float)cell_number), (int)(img2_height / (float)cell_number), 10));
 857         }
 858     }
 859
 860     // update points by calibrated for Essential matrix after graph is calculated
 861     if (params->isEssential()) {
 862         points = calib_points;
 863         // if maximum calibrated threshold significanlty differs threshold then set upper bound
 864         if (max_thr > 10*threshold)
 865             max_thr = sqrt(10*threshold); // max thr will be squared after
 866     }
 867     if (max_thr < threshold)
 868         max_thr = threshold;
 869
 870     switch (params->getError()) {
 871         case ErrorMetric::SYMM_REPR_ERR:
 872             error = ReprojectionErrorSymmetric::create(points); break;
 873         case ErrorMetric::FORW_REPR_ERR:
 874             if (params->getEstimator() == EstimationMethod::Affine)
 875                 error = ReprojectionErrorAffine::create(points);
 876             else error = ReprojectionErrorForward::create(points);
 877             break;
 878         case ErrorMetric::SAMPSON_ERR:
 879             error = SampsonError::create(points); break;
 880         case ErrorMetric::SGD_ERR:
 881             error = SymmetricGeometricDistance::create(points); break;
 882         case ErrorMetric::RERPOJ:
 883             error = ReprojectionErrorPmatrix::create(points); break;
 884         default: CV_Error(cv::Error::StsNotImplemented , "Error metric is not implemented!");
 885     }
 886
 887     switch (params->getScore()) {
 888         case ScoreMethod::SCORE_METHOD_RANSAC :
 889             quality = RansacQuality::create(points_size, threshold, error); break;
 890         case ScoreMethod::SCORE_METHOD_MSAC :
 891             quality = MsacQuality::create(points_size, threshold, error); break;
 892         case ScoreMethod::SCORE_METHOD_MAGSAC :
 893             quality = MagsacQuality::create(max_thr, points_size, error,
 894                 threshold, params->getDegreesOfFreedom(),  params->getSigmaQuantile(),
 895                 params->getUpperIncompleteOfSigmaQuantile(),
 896                 params->getLowerIncompleteOfSigmaQuantile(), params->getC()); break;
 897         case ScoreMethod::SCORE_METHOD_LMEDS :
 898             quality = LMedsQuality::create(points_size, threshold, error); break;
 899         default: CV_Error(cv::Error::StsNotImplemented, "Score is not imeplemeted!");
 900     }
 901
 902     if (params->isHomography()) {
 903         degeneracy = HomographyDegeneracy::create(points);
 904         min_solver = HomographyMinimalSolver4ptsGEM::create(points);
 905         non_min_solver = HomographyNonMinimalSolver::create(points);
 906         estimator = HomographyEstimator::create(min_solver, non_min_solver, degeneracy);
 907     } else if (params->isFundamental()) {
 908         degeneracy = FundamentalDegeneracy::create(state++, quality, points, min_sample_size, 5. /*sqr homogr thr*/);
 909         if(min_sample_size == 7) min_solver = FundamentalMinimalSolver7pts::create(points);
 910         else min_solver = FundamentalMinimalSolver8pts::create(points);
 911         non_min_solver = FundamentalNonMinimalSolver::create(points);
 912         estimator = FundamentalEstimator::create(min_solver, non_min_solver, degeneracy);
 913     } else if (params->isEssential()) {
 914         degeneracy = EssentialDegeneracy::create(points, min_sample_size);
 915         min_solver = EssentialMinimalSolverStewenius5pts::create(points);
 916         non_min_solver = EssentialNonMinimalSolver::create(points);
 917         estimator = EssentialEstimator::create(min_solver, non_min_solver, degeneracy);
 918     } else if (params->isPnP()) {
 919         degeneracy = makePtr<Degeneracy>();
 920         if (min_sample_size == 3) {
 921             non_min_solver = DLSPnP::create(points, calib_points, K1);
 922             min_solver = P3PSolver::create(points, calib_points, K1);
 923         } else {
 924             min_solver = PnPMinimalSolver6Pts::create(points);
 925             non_min_solver = PnPNonMinimalSolver::create(points);
 926         }
 927         estimator = PnPEstimator::create(min_solver, non_min_solver);
 928     } else if (params->getEstimator() == EstimationMethod::Affine) {
 929         degeneracy = makePtr<Degeneracy>();
 930         min_solver = AffineMinimalSolver::create(points);
 931         non_min_solver = AffineNonMinimalSolver::create(points);
 932         estimator = AffineEstimator::create(min_solver, non_min_solver);
 933     } else CV_Error(cv::Error::StsNotImplemented, "Estimator not implemented!");
 934
 935     switch (params->getSampler()) {
 936         case SamplingMethod::SAMPLING_UNIFORM:
 937             sampler = UniformSampler::create(state++, min_sample_size, points_size); break;
 938         case SamplingMethod::SAMPLING_PROSAC:
 939             sampler = ProsacSampler::create(state++, points_size, min_sample_size, 200000); break;
 940         case SamplingMethod::SAMPLING_PROGRESSIVE_NAPSAC:
 941             sampler = ProgressiveNapsac::create(state++, points_size, min_sample_size, layers, 20); break;
 942         case SamplingMethod::SAMPLING_NAPSAC:
 943             sampler = NapsacSampler::create(state++, points_size, min_sample_size, graph); break;
 944         default: CV_Error(cv::Error::StsNotImplemented, "Sampler is not implemented!");
 945     }
 946
 947     switch (params->getVerifier()) {
 948         case VerificationMethod::NullVerifier: verifier = ModelVerifier::create(); break;
 949         case VerificationMethod::SprtVerifier:
 950             verifier = SPRT::create(state++, error, points_size, params->getScore() == ScoreMethod ::SCORE_METHOD_MAGSAC ? max_thr : threshold,
 951              params->getSPRTepsilon(), params->getSPRTdelta(), params->getTimeForModelEstimation(),
 952              params->getSPRTavgNumModels(), params->getScore()); break;
 953         default: CV_Error(cv::Error::StsNotImplemented, "Verifier is not imeplemented!");
 954     }
 955
 956     if (params->getSampler() == SamplingMethod::SAMPLING_PROSAC) {
 957         termination = ProsacTerminationCriteria::create(sampler.dynamicCast<ProsacSampler>(), error,
 958                 points_size, min_sample_size, params->getConfidence(),
 959                 params->getMaxIters(), 100, 0.05, 0.05, threshold);
 960     } else if (params->getSampler() == SamplingMethod::SAMPLING_PROGRESSIVE_NAPSAC) {
 961         if (params->getVerifier() == VerificationMethod::SprtVerifier)
 962             termination = SPRTPNapsacTermination::create(((SPRT *)verifier.get())->getSPRTvector(),
 963                     params->getConfidence(), points_size, min_sample_size,
 964                     params->getMaxIters(), params->getRelaxCoef());
 965         else
 966             termination = StandardTerminationCriteria::create (params->getConfidence(),
 967                     points_size, min_sample_size, params->getMaxIters());
 968     } else if (params->getVerifier() == VerificationMethod::SprtVerifier) {
 969         termination = SPRTTermination::create(((SPRT *) verifier.get())->getSPRTvector(),
 970              params->getConfidence(), points_size, min_sample_size, params->getMaxIters());
 971     } else
 972         termination = StandardTerminationCriteria::create
 973             (params->getConfidence(), points_size, min_sample_size, params->getMaxIters());
 974
 975     if (params->getLO() != LocalOptimMethod::LOCAL_OPTIM_NULL) {
 976         lo_sampler = UniformRandomGenerator::create(state++, points_size, params->getLOSampleSize());
 977         switch (params->getLO()) {
 978             case LocalOptimMethod::LOCAL_OPTIM_INNER_LO:
 979                 lo = InnerIterativeLocalOptimization::create(estimator, quality, lo_sampler,
 980                      points_size, threshold, false, params->getLOIterativeSampleSize(),
 981                      params->getLOInnerMaxIters(), params->getLOIterativeMaxIters(),
 982                      params->getLOThresholdMultiplier()); break;
 983             case LocalOptimMethod::LOCAL_OPTIM_INNER_AND_ITER_LO:
 984                 lo = InnerIterativeLocalOptimization::create(estimator, quality, lo_sampler,
 985                      points_size, threshold, true, params->getLOIterativeSampleSize(),
 986                      params->getLOInnerMaxIters(), params->getLOIterativeMaxIters(),
 987                      params->getLOThresholdMultiplier()); break;
 988             case LocalOptimMethod::LOCAL_OPTIM_GC:
 989                 lo = GraphCut::create(estimator, error, quality, graph, lo_sampler, threshold,
 990                    params->getGraphCutSpatialCoherenceTerm(), params->getLOInnerMaxIters()); break;
 991             case LocalOptimMethod::LOCAL_OPTIM_SIGMA:
 992                 lo = SigmaConsensus::create(estimator, error, quality, verifier,
 993                      params->getLOSampleSize(), params->getLOInnerMaxIters(),
 994                      params->getDegreesOfFreedom(), params->getSigmaQuantile(),
 995                      params->getUpperIncompleteOfSigmaQuantile(), params->getC(), max_thr); break;
 996             default: CV_Error(cv::Error::StsNotImplemented , "Local Optimization is not implemented!");
 997         }
 998     }
 999
1000     if (params->getFinalPolisher() == PolishingMethod::LSQPolisher)
1001         polisher = LeastSquaresPolishing::create(estimator, quality, params->getFinalLSQIterations());
1002
1003     Ransac ransac (params, points_size, estimator, quality, sampler,
1004           termination, verifier, degeneracy, lo, polisher, params->isParallel(), state);
1005     if (ransac.run(ransac_output)) {
1006         if (params->isPnP()) {
1007             // convert R to rodrigues and back and recalculate inliers which due to numerical
1008             // issues can differ
1009             Mat out, R, newR, newP, t, rvec;
1010             if (K1.empty()) {
1011                 usac::Utils::decomposeProjection (ransac_output->getModel(), K1, R, t);
1012                 Rodrigues(R, rvec);
1013                 hconcat(rvec, t, out);
1014                 hconcat(out, K1, out);
1015             } else {
1016                 const Mat Rt = K1.inv() * ransac_output->getModel();
1017                 t = Rt.col(3);
1018                 Rodrigues(Rt.colRange(0,3), rvec);
1019                 hconcat(rvec, t, out);
1020             }
1021             Rodrigues(rvec, newR);
1022             hconcat(K1 * newR, K1 * t, newP);
1023             std::vector<bool> inliers_mask(points_size);
1024             quality->getInliers(newP, inliers_mask);
1025             ransac_output = RansacOutput::create(out, inliers_mask, 0,0,0,0,0,0);
1026         }
1027         return true;
1028     }
1029     return false;
1030 }
1031 }}