diff --git a/RansacLib/ransac.h b/RansacLib/ransac.h index a2f3637..516de95 100644 --- a/RansacLib/ransac.h +++ b/RansacLib/ransac.h @@ -259,7 +259,8 @@ class LocallyOptimizedMSAC : public RansacBase { solver.LeastSquares(stats.inlier_indices, &refined_model); double score = std::numeric_limits::max(); - ScoreModel(solver, refined_model, kSqrInlierThresh, &score); + ScoreModel(solver, refined_model, kSqrInlierThresh, &score, + stats.best_model_score); if (score < stats.best_model_score) { stats.best_model_score = score; *best_model = refined_model; @@ -283,7 +284,8 @@ class LocallyOptimizedMSAC : public RansacBase { *best_model_id = 0; for (int m = 0; m < num_models; ++m) { double score = std::numeric_limits::max(); - ScoreModel(solver, models[m], squared_inlier_threshold, &score); + ScoreModel(solver, models[m], squared_inlier_threshold, &score, + *best_score); if (score < *best_score) { *best_score = score; @@ -293,12 +295,16 @@ class LocallyOptimizedMSAC : public RansacBase { } void ScoreModel(const Solver& solver, const Model& model, - const double squared_inlier_threshold, double* score) const { + const double squared_inlier_threshold, double* score + const double best_score) const { const int kNumData = solver.num_data(); *score = 0.0; for (int i = 0; i < kNumData; ++i) { double squared_error = solver.EvaluateModelOnPoint(model, i); *score += ComputeScore(squared_error, squared_inlier_threshold); + if( *score > best_score) { + return; + } } } @@ -361,7 +367,7 @@ class LocallyOptimizedMSAC : public RansacBase { LeastSquaresFit(options, kSqInThresh * kThreshMult, solver, rng, &m_init); double score = std::numeric_limits::max(); - ScoreModel(solver, m_init, kSqInThresh, &score); + ScoreModel(solver, m_init, kSqInThresh, &score, score_best_minimal_model); UpdateBestModel(score, m_init, score_best_minimal_model, best_minimal_model); @@ -383,7 +389,8 @@ class LocallyOptimizedMSAC : public RansacBase { Model m_non_min; if (!solver.NonMinimalSolver(sample, &m_non_min)) continue; - ScoreModel(solver, m_non_min, kSqInThresh, &score); + ScoreModel(solver, m_non_min, kSqInThresh, &score, + score_best_minimal_model); UpdateBestModel(score, m_non_min, score_best_minimal_model, best_minimal_model); @@ -398,7 +405,8 @@ class LocallyOptimizedMSAC : public RansacBase { for (int i = 0; i < options.num_lsq_iterations_; ++i) { LeastSquaresFit(options, thresh, solver, rng, &m_non_min); - ScoreModel(solver, m_non_min, kSqInThresh, &score); + ScoreModel(solver, m_non_min, kSqInThresh, &score, + score_best_minimal_model); UpdateBestModel(score, m_non_min, score_best_minimal_model, best_minimal_model); thresh -= thresh_mult_update;