modules/calib3d/src/usac/local_optimization.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 "opencv2/imgproc/detail/gcgraph.hpp"
   8
   9 namespace cv { namespace usac {
  10 class GraphCutImpl : public GraphCut {
  11 protected:
  12     const Ptr<NeighborhoodGraph> neighborhood_graph;
  13     const Ptr<Estimator> estimator;
  14     const Ptr<Quality> quality;
  15     const Ptr<RandomGenerator> lo_sampler;
  16     const Ptr<Error> error;
  17
  18     int gc_sample_size, lo_inner_iterations, points_size;
  19     double spatial_coherence, sqr_trunc_thr, one_minus_lambda;
  20
  21     std::vector<int> labeling_inliers;
  22     std::vector<double> energies, weights;
  23     std::vector<bool> used_edges;
  24     std::vector<Mat> gc_models;
  25 public:
  26
  27     // In lo_sampler_ the sample size should be set and be equal gc_sample_size_
  28     GraphCutImpl (const Ptr<Estimator> &estimator_, const Ptr<Error> &error_, const Ptr<Quality> &quality_,
  29               const Ptr<NeighborhoodGraph> &neighborhood_graph_, const Ptr<RandomGenerator> &lo_sampler_,
  30               double threshold_, double spatial_coherence_term, int gc_inner_iteration_number_) :
  31               neighborhood_graph (neighborhood_graph_), estimator (estimator_), quality (quality_),
  32               lo_sampler (lo_sampler_), error (error_) {
  33
  34         points_size = quality_->getPointsSize();
  35         spatial_coherence = spatial_coherence_term;
  36         sqr_trunc_thr =  threshold_ * 2.25; // threshold is already squared
  37         gc_sample_size = lo_sampler_->getSubsetSize();
  38         lo_inner_iterations = gc_inner_iteration_number_;
  39         one_minus_lambda = 1.0 - spatial_coherence;
  40
  41         energies = std::vector<double>(points_size);
  42         labeling_inliers = std::vector<int>(points_size);
  43         used_edges = std::vector<bool>(points_size*points_size);
  44         gc_models = std::vector<Mat> (estimator->getMaxNumSolutionsNonMinimal());
  45     }
  46
  47     bool refineModel (const Mat &best_model, const Score &best_model_score,
  48                       Mat &new_model, Score &new_model_score) override {
  49         if (best_model_score.inlier_number < estimator->getNonMinimalSampleSize())
  50             return false;
  51
  52         // improve best model by non minimal estimation
  53         new_model_score = Score(); // set score to inf (worst case)
  54         best_model.copyTo(new_model);
  55
  56         bool is_best_model_updated = true;
  57         while (is_best_model_updated) {
  58             is_best_model_updated = false;
  59
  60             // Build graph problem. Apply graph cut to G
  61             int labeling_inliers_size = labeling(new_model);
  62             for (int iter = 0; iter < lo_inner_iterations; iter++) {
  63                 // sample to generate min (|I_7m|, |I|)
  64                 int num_of_estimated_models;
  65                 if (labeling_inliers_size > gc_sample_size) {
  66                     // generate random subset in range <0; |I|>
  67                     num_of_estimated_models = estimator->estimateModelNonMinimalSample
  68                             (lo_sampler->generateUniqueRandomSubset(labeling_inliers,
  69                                    labeling_inliers_size), gc_sample_size, gc_models, weights);
  70                 } else {
  71                     if (iter > 0) break; // break inliers are not updated
  72                     num_of_estimated_models = estimator->estimateModelNonMinimalSample
  73                             (labeling_inliers, labeling_inliers_size, gc_models, weights);
  74                 }
  75                 for (int model_idx = 0; model_idx < num_of_estimated_models; model_idx++) {
  76                     const Score gc_temp_score = quality->getScore(gc_models[model_idx]);
  77                     // store the best model from estimated models
  78                     if (gc_temp_score.isBetter(new_model_score)) {
  79                         is_best_model_updated = true;
  80                         new_model_score = gc_temp_score;
  81                         gc_models[model_idx].copyTo(new_model);
  82                     }
  83                 }
  84             } // end of inner GC local optimization
  85         } // end of while loop
  86
  87         return true;
  88     }
  89
  90 private:
  91     // find inliers using graph cut algorithm.
  92     int labeling (const Mat& model) {
  93         const auto &errors = error->getErrors(model);
  94
  95         detail::GCGraph<double> graph;
  96
  97         for (int pt = 0; pt < points_size; pt++)
  98             graph.addVtx();
  99
 100         // The distance and energy for each point
 101         double tmp_squared_distance, energy;
 102
 103         // Estimate the vertex capacities
 104         for (int pt = 0; pt < points_size; pt++) {
 105             tmp_squared_distance = errors[pt];
 106             if (std::isnan(tmp_squared_distance))
 107                 tmp_squared_distance = std::numeric_limits<float>::max();
 108             energy = tmp_squared_distance / sqr_trunc_thr; // Truncated quadratic cost
 109
 110             if (tmp_squared_distance <= sqr_trunc_thr)
 111                 graph.addTermWeights(pt, 0, one_minus_lambda * (1 - energy));
 112             else
 113                 graph.addTermWeights(pt, one_minus_lambda * energy, 0);
 114
 115             energies[pt] = energy > 1 ? 1 : energy;
 116         }
 117
 118         std::fill(used_edges.begin(), used_edges.end(), false);
 119
 120         bool has_edges = false;
 121         // Iterate through all points and set their edges
 122         for (int point_idx = 0; point_idx < points_size; ++point_idx) {
 123             energy = energies[point_idx];
 124
 125             // Iterate through  all neighbors
 126             for (int actual_neighbor_idx : neighborhood_graph->getNeighbors(point_idx)) {
 127                 if (actual_neighbor_idx == point_idx ||
 128                     used_edges[actual_neighbor_idx*points_size + point_idx] ||
 129                     used_edges[point_idx*points_size + actual_neighbor_idx])
 130                     continue;
 131
 132                 used_edges[actual_neighbor_idx*points_size + point_idx] = true;
 133                 used_edges[point_idx*points_size + actual_neighbor_idx] = true;
 134
 135                 double a = (0.5 * (energy + energies[actual_neighbor_idx])) * spatial_coherence,
 136                        b = spatial_coherence, c = spatial_coherence, d = 0;
 137                 graph.addTermWeights(point_idx, d, a);
 138                 b -= a;
 139                 if (b + c < 0)
 140                     continue; // invalid regularity
 141                 if (b < 0) {
 142                     graph.addTermWeights(point_idx, 0, b);
 143                     graph.addTermWeights(actual_neighbor_idx, 0, -b);
 144                     graph.addEdges(point_idx, actual_neighbor_idx, 0, b + c);
 145                 } else if (c < 0) {
 146                     graph.addTermWeights(point_idx, 0, -c);
 147                     graph.addTermWeights(actual_neighbor_idx, 0, c);
 148                     graph.addEdges(point_idx, actual_neighbor_idx, b + c, 0);
 149                 } else
 150                     graph.addEdges(point_idx, actual_neighbor_idx, b, c);
 151                 has_edges = true;
 152             }
 153         }
 154
 155         if (!has_edges)
 156             return quality->getInliers(model, labeling_inliers);
 157
 158         graph.maxFlow();
 159
 160         int inlier_number = 0;
 161         for (int pt = 0; pt < points_size; pt++)
 162             if (! graph.inSourceSegment(pt)) // check for sink
 163                 labeling_inliers[inlier_number++] = pt;
 164         return inlier_number;
 165     }
 166     Ptr<LocalOptimization> clone(int state) const override {
 167         return makePtr<GraphCutImpl>(estimator->clone(), error->clone(), quality->clone(),
 168                 neighborhood_graph,lo_sampler->clone(state), sqr_trunc_thr / 2.25,
 169                 spatial_coherence, lo_inner_iterations);
 170     }
 171 };
 172 Ptr<GraphCut> GraphCut::create(const Ptr<Estimator> &estimator_, const Ptr<Error> &error_,
 173        const Ptr<Quality> &quality_, const Ptr<NeighborhoodGraph> &neighborhood_graph_,
 174        const Ptr<RandomGenerator> &lo_sampler_, double threshold_,
 175        double spatial_coherence_term, int gc_inner_iteration_number) {
 176     return makePtr<GraphCutImpl>(estimator_, error_, quality_, neighborhood_graph_, lo_sampler_,
 177         threshold_, spatial_coherence_term, gc_inner_iteration_number);
 178 }
 179
 180 /*
 181 * http://cmp.felk.cvut.cz/~matas/papers/chum-dagm03.pdf
 182 */
 183 class InnerIterativeLocalOptimizationImpl : public InnerIterativeLocalOptimization {
 184 private:
 185     const Ptr<Estimator> estimator;
 186     const Ptr<Quality> quality;
 187     const Ptr<RandomGenerator> lo_sampler;
 188     Ptr<RandomGenerator> lo_iter_sampler;
 189
 190     std::vector<Mat> lo_models, lo_iter_models;
 191
 192     std::vector<int> inliers_of_best_model, virtual_inliers;
 193     int lo_inner_max_iterations, lo_iter_max_iterations, lo_sample_size, lo_iter_sample_size;
 194
 195     bool is_iterative;
 196
 197     double threshold, new_threshold, threshold_step;
 198     std::vector<double> weights;
 199 public:
 200
 201     InnerIterativeLocalOptimizationImpl (const Ptr<Estimator> &estimator_, const Ptr<Quality> &quality_,
 202          const Ptr<RandomGenerator> &lo_sampler_, int pts_size,
 203          double threshold_, bool is_iterative_, int lo_iter_sample_size_,
 204          int lo_inner_iterations_=10, int lo_iter_max_iterations_=5,
 205          double threshold_multiplier_=4)
 206         : estimator (estimator_), quality (quality_), lo_sampler (lo_sampler_)
 207         , lo_iter_sample_size(0)
 208         , new_threshold(0), threshold_step(0)
 209     {
 210         lo_inner_max_iterations = lo_inner_iterations_;
 211         lo_iter_max_iterations = lo_iter_max_iterations_;
 212
 213         threshold = threshold_;
 214
 215         lo_sample_size = lo_sampler->getSubsetSize();
 216
 217         is_iterative = is_iterative_;
 218         if (is_iterative) {
 219             lo_iter_sample_size = lo_iter_sample_size_;
 220             lo_iter_sampler = UniformRandomGenerator::create(0/*state*/, pts_size, lo_iter_sample_size_);
 221             lo_iter_models = std::vector<Mat>(estimator->getMaxNumSolutionsNonMinimal());
 222             virtual_inliers = std::vector<int>(pts_size);
 223             new_threshold = threshold_multiplier_ * threshold;
 224             // reduce multiplier threshold K·θ by this number in each iteration.
 225             // In the last iteration there be original threshold θ.
 226             threshold_step = (new_threshold - threshold) / lo_iter_max_iterations_;
 227         }
 228
 229         lo_models = std::vector<Mat>(estimator->getMaxNumSolutionsNonMinimal());
 230
 231         // Allocate max memory to avoid reallocation
 232         inliers_of_best_model = std::vector<int>(pts_size);
 233     }
 234
 235     /*
 236      * Implementation of Locally Optimized Ransac
 237      * Inner + Iterative
 238      */
 239     bool refineModel (const Mat &so_far_the_best_model, const Score &best_model_score,
 240                       Mat &new_model, Score &new_model_score) override {
 241         if (best_model_score.inlier_number < estimator->getNonMinimalSampleSize())
 242             return false;
 243
 244         so_far_the_best_model.copyTo(new_model);
 245         new_model_score = best_model_score;
 246         // get inliers from so far the best model.
 247         int num_inliers_of_best_model = quality->getInliers(so_far_the_best_model,
 248                                                            inliers_of_best_model);
 249
 250         // Inner Local Optimization Ransac.
 251         for (int iters = 0; iters < lo_inner_max_iterations; iters++) {
 252             int num_estimated_models;
 253             // Generate sample of lo_sample_size from inliers from the best model.
 254             if (num_inliers_of_best_model > lo_sample_size) {
 255                 // if there are many inliers take limited number at random.
 256                 num_estimated_models = estimator->estimateModelNonMinimalSample
 257                         (lo_sampler->generateUniqueRandomSubset(inliers_of_best_model,
 258                                 num_inliers_of_best_model), lo_sample_size, lo_models, weights);
 259             } else {
 260                 // if model was not updated in first iteration, so break.
 261                 if (iters > 0) break;
 262                 // if inliers are less than limited number of sample then take all for estimation
 263                 // if it fails -> end Lo.
 264                 num_estimated_models = estimator->estimateModelNonMinimalSample
 265                     (inliers_of_best_model, num_inliers_of_best_model, lo_models, weights);
 266             }
 267
 268             //////// Choose the best lo_model from estimated lo_models.
 269             for (int model_idx = 0; model_idx < num_estimated_models; model_idx++) {
 270                 const Score temp_score = quality->getScore(lo_models[model_idx]);
 271                 if (temp_score.isBetter(new_model_score)) {
 272                     new_model_score = temp_score;
 273                     lo_models[model_idx].copyTo(new_model);
 274                 }
 275             }
 276
 277             if (is_iterative) {
 278                 double lo_threshold = new_threshold;
 279                 // get max virtual inliers. Note that they are nor real inliers,
 280                 // because we got them with bigger threshold.
 281                 int virtual_inliers_size = quality->getInliers
 282                         (new_model, virtual_inliers, lo_threshold);
 283
 284                 Mat lo_iter_model;
 285                 Score lo_iter_score = Score(); // set worst case
 286                 for (int iterations = 0; iterations < lo_iter_max_iterations; iterations++) {
 287                     lo_threshold -= threshold_step;
 288
 289                     if (virtual_inliers_size > lo_iter_sample_size) {
 290                         // if there are more inliers than limit for sample size then generate at random
 291                         // sample from LO model.
 292                         num_estimated_models = estimator->estimateModelNonMinimalSample
 293                                 (lo_iter_sampler->generateUniqueRandomSubset (virtual_inliers,
 294                             virtual_inliers_size), lo_iter_sample_size, lo_iter_models, weights);
 295                     } else {
 296                         // break if failed, very low probability that it will not fail in next iterations
 297                         // estimate model with all virtual inliers
 298                         num_estimated_models = estimator->estimateModelNonMinimalSample
 299                                 (virtual_inliers, virtual_inliers_size, lo_iter_models, weights);
 300                     }
 301                     if (num_estimated_models == 0) break;
 302
 303                     // Get score and update virtual inliers with current threshold
 304                     ////// Choose the best lo_iter_model from estimated lo_iter_models.
 305                     lo_iter_models[0].copyTo(lo_iter_model);
 306                     lo_iter_score = quality->getScore(lo_iter_model);
 307                     for (int model_idx = 1; model_idx < num_estimated_models; model_idx++) {
 308                         const Score temp_score = quality->getScore(lo_iter_models[model_idx]);
 309                         if (temp_score.isBetter(lo_iter_score)) {
 310                             lo_iter_score = temp_score;
 311                             lo_iter_models[model_idx].copyTo(lo_iter_model);
 312                         }
 313                     }
 314
 315                     if (iterations != lo_iter_max_iterations-1)
 316                         virtual_inliers_size = quality->getInliers(lo_iter_model, virtual_inliers, lo_threshold);
 317                 }
 318
 319                 if (lo_iter_score.isBetter(new_model_score)) {
 320                     new_model_score = lo_iter_score;
 321                     lo_iter_model.copyTo(new_model);
 322                 }
 323             }
 324
 325             if (num_inliers_of_best_model < new_model_score.inlier_number && iters != lo_inner_max_iterations-1)
 326                 num_inliers_of_best_model = quality->getInliers (new_model, inliers_of_best_model);
 327         }
 328         return true;
 329     }
 330     Ptr<LocalOptimization> clone(int state) const override {
 331         return makePtr<InnerIterativeLocalOptimizationImpl>(estimator->clone(), quality->clone(),
 332             lo_sampler->clone(state),(int)inliers_of_best_model.size(), threshold, is_iterative,
 333             lo_iter_sample_size, lo_inner_max_iterations, lo_iter_max_iterations,
 334             new_threshold / threshold);
 335     }
 336 };
 337 Ptr<InnerIterativeLocalOptimization> InnerIterativeLocalOptimization::create
 338 (const Ptr<Estimator> &estimator_, const Ptr<Quality> &quality_,
 339        const Ptr<RandomGenerator> &lo_sampler_, int pts_size,
 340        double threshold_, bool is_iterative_, int lo_iter_sample_size_,
 341        int lo_inner_iterations_, int lo_iter_max_iterations_,
 342        double threshold_multiplier_) {
 343     return makePtr<InnerIterativeLocalOptimizationImpl>(estimator_, quality_, lo_sampler_,
 344             pts_size, threshold_, is_iterative_, lo_iter_sample_size_,
 345             lo_inner_iterations_, lo_iter_max_iterations_, threshold_multiplier_);
 346 }
 347
 348 class SigmaConsensusImpl : public SigmaConsensus {
 349 private:
 350     const Ptr<Estimator> estimator;
 351     const Ptr<Quality> quality;
 352     const Ptr<Error> error;
 353     const Ptr<ModelVerifier> verifier;
 354     const GammaValues& gamma_generator;
 355     // The degrees of freedom of the data from which the model is estimated.
 356     // E.g., for models coming from point correspondences (x1,y1,x2,y2), it is 4.
 357     const int degrees_of_freedom;
 358     // A 0.99 quantile of the Chi^2-distribution to convert sigma values to residuals
 359     const double k;
 360     // Calculating (DoF - 1) / 2 which will be used for the estimation and,
 361     // due to being constant, it is better to calculate it a priori.
 362     double dof_minus_one_per_two;
 363     const double C;
 364     // The size of a minimal sample used for the estimation
 365     const int sample_size;
 366     // Calculating 2^(DoF - 1) which will be used for the estimation and,
 367     // due to being constant, it is better to calculate it a priori.
 368     double two_ad_dof;
 369     // Calculating C * 2^(DoF - 1) which will be used for the estimation and,
 370     // due to being constant, it is better to calculate it a priori.
 371     double C_times_two_ad_dof;
 372     // Calculating the gamma value of (DoF - 1) / 2 which will be used for the estimation and,
 373     // due to being constant, it is better to calculate it a priori.
 374     double squared_sigma_max_2, one_over_sigma;
 375     // Calculating the upper incomplete gamma value of (DoF - 1) / 2 with k^2 / 2.
 376     const double gamma_k;
 377     // Calculating the lower incomplete gamma value of (DoF - 1) / 2 which will be used for the estimation and,
 378     // due to being constant, it is better to calculate it a priori.
 379     double max_sigma_sqr;
 380     const int points_size, number_of_irwls_iters;
 381     const double maximum_threshold, max_sigma;
 382
 383     std::vector<double> sqr_residuals, sigma_weights;
 384     std::vector<int> sqr_residuals_idxs;
 385     // Models fit by weighted least-squares fitting
 386     std::vector<Mat> sigma_models;
 387     // Points used in the weighted least-squares fitting
 388     std::vector<int> sigma_inliers;
 389     // Weights used in the the weighted least-squares fitting
 390     int max_lo_sample_size, stored_gamma_number_min1;
 391     double scale_of_stored_gammas;
 392     RNG rng;
 393     const std::vector<double> &stored_gamma_values;
 394 public:
 395
 396     SigmaConsensusImpl (const Ptr<Estimator> &estimator_, const Ptr<Error> &error_,
 397         const Ptr<Quality> &quality_, const Ptr<ModelVerifier> &verifier_,
 398         int max_lo_sample_size_, int number_of_irwls_iters_, int DoF,
 399         double sigma_quantile, double upper_incomplete_of_sigma_quantile, double C_,
 400         double maximum_thr) : estimator (estimator_), quality(quality_),
 401           error (error_), verifier(verifier_),
 402           gamma_generator(GammaValues::getSingleton()),
 403           degrees_of_freedom(DoF), k (sigma_quantile), C(C_),
 404           sample_size(estimator_->getMinimalSampleSize()),
 405           gamma_k (upper_incomplete_of_sigma_quantile), points_size (quality_->getPointsSize()),
 406           number_of_irwls_iters (number_of_irwls_iters_),
 407           maximum_threshold(maximum_thr), max_sigma (maximum_thr),
 408           stored_gamma_values(gamma_generator.getGammaValues())
 409     {
 410         dof_minus_one_per_two = (degrees_of_freedom - 1.0) / 2.0;
 411         two_ad_dof = std::pow(2.0, dof_minus_one_per_two);
 412         C_times_two_ad_dof = C * two_ad_dof;
 413         // Calculate 2 * \sigma_{max}^2 a priori
 414         squared_sigma_max_2 = max_sigma * max_sigma * 2.0;
 415         // Divide C * 2^(DoF - 1) by \sigma_{max} a priori
 416         one_over_sigma = C_times_two_ad_dof / max_sigma;
 417         max_sigma_sqr = squared_sigma_max_2 * 0.5;
 418         sqr_residuals = std::vector<double>(points_size);
 419         sqr_residuals_idxs = std::vector<int>(points_size);
 420         sigma_inliers = std::vector<int>(points_size);
 421         max_lo_sample_size = max_lo_sample_size_;
 422         sigma_weights = std::vector<double>(points_size);
 423         sigma_models = std::vector<Mat>(estimator->getMaxNumSolutionsNonMinimal());
 424         stored_gamma_number_min1 = gamma_generator.getTableSize()-1;
 425         scale_of_stored_gammas = gamma_generator.getScaleOfGammaValues();
 426     }
 427
 428     // https://github.com/danini/magsac
 429     bool refineModel (const Mat &in_model, const Score &best_model_score,
 430                       Mat &new_model, Score &new_model_score) override {
 431         int residual_cnt = 0;
 432
 433         if (verifier->isModelGood(in_model)) {
 434             if (verifier->hasErrors()) {
 435                 const std::vector<float> &errors = verifier->getErrors();
 436                 for (int point_idx = 0; point_idx < points_size; ++point_idx) {
 437                     // Calculate the residual of the current point
 438                     const auto residual = sqrtf(errors[point_idx]);
 439                     if (max_sigma > residual) {
 440                         // Store the residual of the current point and its index
 441                         sqr_residuals[residual_cnt] = residual;
 442                         sqr_residuals_idxs[residual_cnt++] = point_idx;
 443                     }
 444
 445                     // Interrupt if there is no chance of being better
 446                     if (residual_cnt + points_size - point_idx < best_model_score.inlier_number)
 447                         return false;
 448                 }
 449             } else {
 450                 error->setModelParameters(in_model);
 451
 452                 for (int point_idx = 0; point_idx < points_size; ++point_idx) {
 453                     const double sqr_residual = error->getError(point_idx);
 454                     if (sqr_residual < max_sigma_sqr) {
 455                         // Store the residual of the current point and its index
 456                         sqr_residuals[residual_cnt] = sqr_residual;
 457                         sqr_residuals_idxs[residual_cnt++] = point_idx;
 458                     }
 459
 460                     if (residual_cnt + points_size - point_idx < best_model_score.inlier_number)
 461                         return false;
 462                 }
 463             }
 464         } else return false;
 465
 466         in_model.copyTo(new_model);
 467         new_model_score = Score();
 468
 469         // Do the iteratively re-weighted least squares fitting
 470         for (int iterations = 0; iterations < number_of_irwls_iters; iterations++) {
 471             int sigma_inliers_cnt = 0;
 472             // If the current iteration is not the first, the set of possibly inliers
 473             // (i.e., points closer than the maximum threshold) have to be recalculated.
 474             if (iterations > 0) {
 475                 // error->setModelParameters(polished_model);
 476                 error->setModelParameters(new_model);
 477                 // Remove everything from the residual vector
 478                 residual_cnt = 0;
 479
 480                 // Collect the points which are closer than the maximum threshold
 481                 for (int point_idx = 0; point_idx < points_size; ++point_idx) {
 482                     // Calculate the residual of the current point
 483                     const double sqr_residual = error->getError(point_idx);
 484                     if (sqr_residual < max_sigma_sqr) {
 485                         // Store the residual of the current point and its index
 486                         sqr_residuals[residual_cnt] = sqr_residual;
 487                         sqr_residuals_idxs[residual_cnt++] = point_idx;
 488                     }
 489                 }
 490                 sigma_inliers_cnt = 0;
 491             }
 492
 493             // Calculate the weight of each point
 494             for (int i = 0; i < residual_cnt; i++) {
 495                 // Get the position of the gamma value in the lookup table
 496                 int x = (int)round(scale_of_stored_gammas * sqr_residuals[i]
 497                         / squared_sigma_max_2);
 498
 499                 // If the sought gamma value is not stored in the lookup, return the closest element
 500                 if (x >= stored_gamma_number_min1 || x < 0 /*overflow*/) // actual number of gamma values is 1 more, so >=
 501                     x  = stored_gamma_number_min1;
 502
 503                 sigma_inliers[sigma_inliers_cnt] = sqr_residuals_idxs[i]; // store index of point for LSQ
 504                 sigma_weights[sigma_inliers_cnt++] = one_over_sigma * (stored_gamma_values[x] - gamma_k);
 505             }
 506
 507             // random shuffle sigma inliers
 508             if (sigma_inliers_cnt > max_lo_sample_size)
 509                 for (int i = sigma_inliers_cnt-1; i > 0; i--) {
 510                     const int idx = rng.uniform(0, i+1);
 511                     std::swap(sigma_inliers[i], sigma_inliers[idx]);
 512                     std::swap(sigma_weights[i], sigma_weights[idx]);
 513                 }
 514             const int num_est_models = estimator->estimateModelNonMinimalSample
 515                   (sigma_inliers, std::min(max_lo_sample_size, sigma_inliers_cnt),
 516                           sigma_models, sigma_weights);
 517
 518             if (num_est_models == 0)
 519                 break; // break iterations
 520
 521             // Update the model parameters
 522             Mat polished_model = sigma_models[0];
 523             if (num_est_models > 1) {
 524                 // find best over other models
 525                 Score sigma_best_score = quality->getScore(polished_model);
 526                 for (int m = 1; m < num_est_models; m++) {
 527                     const Score sc = quality->getScore(sigma_models[m]);
 528                     if (sc.isBetter(sigma_best_score)) {
 529                         polished_model = sigma_models[m];
 530                         sigma_best_score = sc;
 531                     }
 532                 }
 533             }
 534
 535             const Score polished_model_score = quality->getScore(polished_model);
 536             if (polished_model_score.isBetter(new_model_score)){
 537                 new_model_score = polished_model_score;
 538                 polished_model.copyTo(new_model);
 539             }
 540         }
 541
 542         const Score in_model_score = quality->getScore(in_model);
 543         if (in_model_score.isBetter(new_model_score)) {
 544             new_model_score = in_model_score;
 545             in_model.copyTo(new_model);
 546         }
 547
 548         return true;
 549     }
 550     Ptr<LocalOptimization> clone(int state) const override {
 551         return makePtr<SigmaConsensusImpl>(estimator->clone(), error->clone(), quality->clone(),
 552                 verifier->clone(state), max_lo_sample_size,
 553                 number_of_irwls_iters, degrees_of_freedom, k, gamma_k, C, maximum_threshold);
 554     }
 555 };
 556 Ptr<SigmaConsensus>
 557 SigmaConsensus::create(const Ptr<Estimator> &estimator_, const Ptr<Error> &error_,
 558         const Ptr<Quality> &quality, const Ptr<ModelVerifier> &verifier_,
 559         int max_lo_sample_size, int number_of_irwls_iters_, int DoF,
 560         double sigma_quantile, double upper_incomplete_of_sigma_quantile, double C_,
 561         double maximum_thr) {
 562     return makePtr<SigmaConsensusImpl>(estimator_, error_, quality, verifier_,
 563             max_lo_sample_size, number_of_irwls_iters_, DoF, sigma_quantile,
 564             upper_incomplete_of_sigma_quantile, C_, maximum_thr);
 565 }
 566
 567 /////////////////////////////////////////// FINAL MODEL POLISHER ////////////////////////
 568 class LeastSquaresPolishingImpl : public LeastSquaresPolishing {
 569 private:
 570     const Ptr<Estimator> estimator;
 571     const Ptr<Quality> quality;
 572     int lsq_iterations;
 573     std::vector<int> inliers;
 574     std::vector<Mat> models;
 575     std::vector<double> weights;
 576 public:
 577
 578     LeastSquaresPolishingImpl(const Ptr<Estimator> &estimator_, const Ptr<Quality> &quality_,
 579             int lsq_iterations_) :
 580             estimator(estimator_), quality(quality_) {
 581         lsq_iterations = lsq_iterations_;
 582         // allocate memory for inliers array and models
 583         inliers = std::vector<int>(quality_->getPointsSize());
 584         models = std::vector<Mat>(estimator->getMaxNumSolutionsNonMinimal());
 585     }
 586
 587     bool polishSoFarTheBestModel(const Mat &model, const Score &best_model_score,
 588                                  Mat &new_model, Score &out_score) override {
 589         // get inliers from input model
 590         int inlier_number = quality->getInliers(model, inliers);
 591         if (inlier_number < estimator->getMinimalSampleSize())
 592             return false;
 593
 594         out_score = Score(); // set the worst case
 595
 596         // several all-inlier least-squares refines model better than only one but for
 597         // big amount of points may be too time-consuming.
 598         for (int lsq_iter = 0; lsq_iter < lsq_iterations; lsq_iter++) {
 599             bool model_updated = false;
 600
 601             // estimate non minimal models with all inliers
 602             const int num_models = estimator->estimateModelNonMinimalSample(inliers,
 603                                                       inlier_number, models, weights);
 604             for (int model_idx = 0; model_idx < num_models; model_idx++) {
 605                 const Score score = quality->getScore(models[model_idx]);
 606                 if (best_model_score.isBetter(score))
 607                     continue;
 608                 if (score.isBetter(out_score)) {
 609                     models[model_idx].copyTo(new_model);
 610                     out_score = score;
 611                     model_updated = true;
 612                 }
 613             }
 614
 615             if (!model_updated)
 616                 // if model was not updated at the first iteration then return false
 617                 // otherwise if all-inliers LSQ has not updated model then no sense
 618                 // to do it again -> return true (model was updated before).
 619                 return lsq_iter > 0;
 620
 621             // if number of inliers doesn't increase more than 5% then break
 622             if (fabs(static_cast<double>(out_score.inlier_number) - static_cast<double>
 623                  (best_model_score.inlier_number)) / best_model_score.inlier_number < 0.05)
 624                 return true;
 625
 626             if (lsq_iter != lsq_iterations - 1)
 627                 // if not the last LSQ normalization then get inliers for next normalization
 628                 inlier_number = quality->getInliers(new_model, inliers);
 629         }
 630         return true;
 631     }
 632 };
 633 Ptr<LeastSquaresPolishing> LeastSquaresPolishing::create (const Ptr<Estimator> &estimator_,
 634          const Ptr<Quality> &quality_, int lsq_iterations_) {
 635     return makePtr<LeastSquaresPolishingImpl>(estimator_, quality_, lsq_iterations_);
 636 }
 637 }}