diff --git a/modules/3d/src/usac.hpp b/modules/3d/src/usac.hpp index 796666ade0..f3dc55b8d7 100644 --- a/modules/3d/src/usac.hpp +++ b/modules/3d/src/usac.hpp @@ -210,12 +210,12 @@ public: class Score { public: int inlier_number; - double score; + float score; Score () { // set worst case inlier_number = 0; - score = std::numeric_limits::max(); + score = std::numeric_limits::max(); } - Score (int inlier_number_, double score_) { // copy constructor + Score (int inlier_number_, float score_) { // copy constructor inlier_number = inlier_number_; score = score_; } @@ -254,7 +254,7 @@ public: // get @inliers of the @model for given threshold virtual int getInliers (const Mat &model, std::vector &inliers, double thr) const = 0; // Set the best score, so evaluation of the model can terminate earlier - virtual void setBestScore (double best_score_) = 0; + virtual void setBestScore (float best_score_) = 0; // set @inliers_mask: true if point i is inlier, false - otherwise. virtual int getInliers (const Mat &model, std::vector &inliers_mask) const = 0; virtual int getPointsSize() const = 0; diff --git a/modules/3d/src/usac/essential_solver.cpp b/modules/3d/src/usac/essential_solver.cpp index 434db6d373..6f0e6b1cfc 100644 --- a/modules/3d/src/usac/essential_solver.cpp +++ b/modules/3d/src/usac/essential_solver.cpp @@ -175,6 +175,8 @@ public: Matx33d Bz(bz); // Bz is rank 2, matrix, so epipole is its null-vector Vec3d xy1 = Utils::getRightEpipole(Mat(Bz * (1/sqrt(norm_bz)))); + const double one_over_xy1_norm = 1 / sqrt(xy1[0] * xy1[0] + xy1[1] * xy1[1] + xy1[2] * xy1[2]); + xy1 *= one_over_xy1_norm; if (fabs(xy1(2)) < 1e-10) continue; Mat_ E(3,3); diff --git a/modules/3d/src/usac/quality.cpp b/modules/3d/src/usac/quality.cpp index 89c5760c1d..9a72f754f5 100644 --- a/modules/3d/src/usac/quality.cpp +++ b/modules/3d/src/usac/quality.cpp @@ -69,7 +69,7 @@ public: else if (inlier_number - point < preemptive_thr) break; // score is negative inlier number! If less then better - return {inlier_number, -static_cast(inlier_number)}; + return {inlier_number, -static_cast(inlier_number)}; } Score getScore (const std::vector &errors) const override { @@ -78,10 +78,10 @@ public: if (errors[point] < threshold) inlier_number++; // score is negative inlier number! If less then better - return {inlier_number, -static_cast(inlier_number)}; + return {inlier_number, -static_cast(inlier_number)}; } - void setBestScore(double best_score_) override { + void setBestScore(float best_score_) override { if (best_score > best_score_) best_score = best_score_; } @@ -106,18 +106,17 @@ protected: const Ptr error; const int points_size; const double threshold, k_msac; - double best_score, norm_thr, one_over_thr; + const float norm_thr, one_over_thr; + float best_score; public: MsacQualityImpl (int points_size_, double threshold_, const Ptr &error_, double k_msac_) - : error (error_), points_size (points_size_), threshold (threshold_), k_msac(k_msac_) { - best_score = std::numeric_limits::max(); - norm_thr = threshold*k_msac; - one_over_thr = 1 / norm_thr; - } + : error (error_), points_size (points_size_), threshold (threshold_), k_msac(k_msac_), + norm_thr(static_cast(threshold*k_msac)), one_over_thr(1.f/norm_thr), + best_score(std::numeric_limits::max()) {} inline Score getScore (const Mat &model) const override { error->setModelParameters(model); - double err, sum_errors = 0; + float err, sum_errors = 0; int inlier_number = 0; const auto preemptive_thr = points_size + best_score; for (int point = 0; point < points_size; point++) { @@ -133,7 +132,7 @@ public: } Score getScore (const std::vector &errors) const override { - double sum_errors = 0; + float sum_errors = 0; int inlier_number = 0; for (int point = 0; point < points_size; point++) { const auto err = errors[point]; @@ -146,7 +145,7 @@ public: return {inlier_number, sum_errors}; } - void setBestScore(double best_score_) override { + void setBestScore(float best_score_) override { if (best_score > best_score_) best_score = best_score_; } @@ -244,7 +243,7 @@ public: } else if (total_loss + point_idx > preemptive_thr) break; } - return {num_tentative_inliers, total_loss}; + return {num_tentative_inliers, (float)total_loss}; } Score getScore (const std::vector &errors) const override { @@ -263,10 +262,10 @@ public: (stored_complete_gamma_values[x] - gamma_value_of_k)) * norm_loss); } } - return {num_tentative_inliers, total_loss}; + return {num_tentative_inliers, (float)total_loss}; } - void setBestScore (double best_loss) override { + void setBestScore (float best_loss) override { if (previous_best_loss > best_loss) previous_best_loss = best_loss; } @@ -317,7 +316,7 @@ public: return {inlier_number, Utils::findMedian (errors)}; } - void setBestScore (double /*best_score*/) override {} + void setBestScore (float /*best_score*/) override {} int getPointsSize () const override { return points_size; } int getInliers (const Mat &model, std::vector &inliers) const override @@ -487,9 +486,9 @@ public: if (last_model_is_good && do_sprt) { out_score.inlier_number = tested_inliers; if (score_type == ScoreMethod::SCORE_METHOD_MSAC) - out_score.score = sum_errors; + out_score.score = static_cast(sum_errors); else if (score_type == ScoreMethod::SCORE_METHOD_RANSAC) - out_score.score = -static_cast(tested_inliers); + out_score.score = -static_cast(tested_inliers); else out_score = quality->getScore(errors); } return last_model_is_good; diff --git a/modules/3d/src/usac/ransac_solvers.cpp b/modules/3d/src/usac/ransac_solvers.cpp index 35c09950cc..84a31682e1 100644 --- a/modules/3d/src/usac/ransac_solvers.cpp +++ b/modules/3d/src/usac/ransac_solvers.cpp @@ -818,7 +818,7 @@ bool UniversalRANSAC::run(Ptr &ransac_output) { const bool is_prosac = params->getSampler() == SamplingMethod::SAMPLING_PROSAC; std::atomic_bool success(false); std::atomic_int num_hypothesis_tested(0), thread_cnt(0), max_number_inliers(0), subset_size, termination_length; - std::atomic best_score_all(std::numeric_limits::max()); + std::atomic best_score_all(std::numeric_limits::max()); std::vector best_scores(MAX_THREADS), best_scores_not_LO; std::vector best_models(MAX_THREADS), best_models_not_LO, K1_apx, K2_apx; std::vector num_tested_models_threads(MAX_THREADS), growth_function, non_random_inliers; @@ -867,7 +867,7 @@ bool UniversalRANSAC::run(Ptr &ransac_output) { model_verifier, local_optimization, termination, sampler, lo_sampler, weight_fnc, true); bool is_last_from_LO_thread = false; Mat best_model_thread, non_degenerate_model, lo_model, best_not_LO_thread; - Score best_score_thread, current_score, non_denegenerate_model_score, lo_score,best_score_all_threads, best_not_LO_score_thread; + Score best_score_thread, current_score, non_denegenerate_model_score, lo_score, best_score_all_threads, best_not_LO_score_thread; std::vector sample(estimator->getMinimalSampleSize()), best_sample_thread, supports; supports.reserve(3*MAX_MODELS_ADAPT); // store model supports std::vector best_inliers_mask_local(points_size, false), model_inliers_mask(points_size, false); @@ -875,7 +875,8 @@ bool UniversalRANSAC::run(Ptr &ransac_output) { auto update_best = [&] (const Score &new_score, const Mat &new_model, bool from_LO=false) { // update best score of all threads if (max_number_inliers < new_score.inlier_number) max_number_inliers = new_score.inlier_number; - if (best_score_all > new_score.score) best_score_all = new_score.score; + if (best_score_all > new_score.score) + best_score_all = new_score.score; best_score_all_threads = Score(max_number_inliers, best_score_all); // quality->getInliers(new_model, model_inliers_mask); @@ -924,7 +925,7 @@ bool UniversalRANSAC::run(Ptr &ransac_output) { success = num_hypothesis_tested++ > max_iters; if (iters % 10 && !adapt) { // Synchronize threads. just to speed verification of model. - quality->setBestScore(std::min(best_score_thread.score, (double)best_score_all)); + quality->setBestScore(std::min(best_score_thread.score, (float)best_score_all)); model_verifier->update(best_score_thread.inlier_number > max_number_inliers ? best_score_thread : best_score_all_threads, iters); }