Port of Replace double atomic in USAC

Port of solution for:
- https://github.com/opencv/opencv/issues/24482
- https://github.com/opencv/opencv/issues/24281

Original PR: https://github.com/opencv/opencv/pull/24499

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<double>::max();
+        score = std::numeric_limits<float>::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<int> &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<bool> &inliers_mask) const = 0;
     virtual int getPointsSize() const = 0;

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_<double> E(3,3);

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<double>(inlier_number)};
+        return {inlier_number, -static_cast<float>(inlier_number)};
     }
 
     Score getScore (const std::vector<float> &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<double>(inlier_number)};
+        return {inlier_number, -static_cast<float>(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> 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> &error_, double k_msac_)
-            : error (error_), points_size (points_size_), threshold (threshold_), k_msac(k_msac_) {
-        best_score = std::numeric_limits<double>::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<float>(threshold*k_msac)), one_over_thr(1.f/norm_thr),
+              best_score(std::numeric_limits<float>::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<float> &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<float> &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<int> &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<float>(sum_errors);
             else if (score_type == ScoreMethod::SCORE_METHOD_RANSAC)
-                out_score.score = -static_cast<double>(tested_inliers);
+                out_score.score = -static_cast<float>(tested_inliers);
             else out_score = quality->getScore(errors);
         }
         return last_model_is_good;

modules/3d/src/usac/ransac_solvers.cpp

@@ -818,7 +818,7 @@ bool UniversalRANSAC::run(Ptr<RansacOutput> &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<double> best_score_all(std::numeric_limits<double>::max());
+        std::atomic<float> best_score_all(std::numeric_limits<float>::max());
         std::vector<Score> best_scores(MAX_THREADS), best_scores_not_LO;
         std::vector<Mat> best_models(MAX_THREADS), best_models_not_LO, K1_apx, K2_apx;
         std::vector<int> num_tested_models_threads(MAX_THREADS), growth_function, non_random_inliers;
@@ -867,7 +867,7 @@ bool UniversalRANSAC::run(Ptr<RansacOutput> &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<int> sample(estimator->getMinimalSampleSize()), best_sample_thread, supports;
             supports.reserve(3*MAX_MODELS_ADAPT); // store model supports
             std::vector<bool> best_inliers_mask_local(points_size, false), model_inliers_mask(points_size, false);
@@ -875,7 +875,8 @@ bool UniversalRANSAC::run(Ptr<RansacOutput> &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<RansacOutput> &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);
                 }