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_
GraphCutImpl(const Ptr<Estimator> & estimator_,const Ptr<Error> & error_,const Ptr<Quality> & quality_,const Ptr<NeighborhoodGraph> & neighborhood_graph_,const Ptr<RandomGenerator> & lo_sampler_,double threshold_,double spatial_coherence_term,int gc_inner_iteration_number_)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
refineModel(const Mat & best_model,const Score & best_model_score,Mat & new_model,Score & new_model_score)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.
labeling(const Mat & model)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 }
clone(int state) const166 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 };
create(const Ptr<Estimator> & estimator_,const Ptr<Error> & error_,const Ptr<Quality> & quality_,const Ptr<NeighborhoodGraph> & neighborhood_graph_,const Ptr<RandomGenerator> & lo_sampler_,double threshold_,double spatial_coherence_term,int gc_inner_iteration_number)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
InnerIterativeLocalOptimizationImpl(const Ptr<Estimator> & estimator_,const Ptr<Quality> & quality_,const Ptr<RandomGenerator> & lo_sampler_,int pts_size,double threshold_,bool is_iterative_,int lo_iter_sample_size_,int lo_inner_iterations_=10,int lo_iter_max_iterations_=5,double threshold_multiplier_=4)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 */
refineModel(const Mat & so_far_the_best_model,const Score & best_model_score,Mat & new_model,Score & new_model_score)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 }
clone(int state) const330 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 };
create(const Ptr<Estimator> & estimator_,const Ptr<Quality> & quality_,const Ptr<RandomGenerator> & lo_sampler_,int pts_size,double threshold_,bool is_iterative_,int lo_iter_sample_size_,int lo_inner_iterations_,int lo_iter_max_iterations_,double threshold_multiplier_)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
SigmaConsensusImpl(const Ptr<Estimator> & estimator_,const Ptr<Error> & error_,const Ptr<Quality> & quality_,const Ptr<ModelVerifier> & verifier_,int max_lo_sample_size_,int number_of_irwls_iters_,int DoF,double sigma_quantile,double upper_incomplete_of_sigma_quantile,double C_,double maximum_thr)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
refineModel(const Mat & in_model,const Score & best_model_score,Mat & new_model,Score & new_model_score)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 }
clone(int state) const550 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>
create(const Ptr<Estimator> & estimator_,const Ptr<Error> & error_,const Ptr<Quality> & quality,const Ptr<ModelVerifier> & verifier_,int max_lo_sample_size,int number_of_irwls_iters_,int DoF,double sigma_quantile,double upper_incomplete_of_sigma_quantile,double C_,double maximum_thr)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
LeastSquaresPolishingImpl(const Ptr<Estimator> & estimator_,const Ptr<Quality> & quality_,int lsq_iterations_)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
polishSoFarTheBestModel(const Mat & model,const Score & best_model_score,Mat & new_model,Score & out_score)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 };
create(const Ptr<Estimator> & estimator_,const Ptr<Quality> & quality_,int lsq_iterations_)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 }}
638