Merge pull request #24499 from ivashmak:usac_bug_fix

Replace double atomic in USAC #24499

### Pull Request Readiness Checklist

See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request

- [x] I agree to contribute to the project under Apache 2 License.
- [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV
- [x] The PR is proposed to the proper branch
- [x] There is a reference to the original bug report and related work
- [x] There is accuracy test, performance test and test data in opencv_extra repository, if applicable
      Patch to opencv_extra has the same branch name.
- [x] The feature is well documented and sample code can be built with the project CMake

Reference to issue with atomic variable: #24281
Reference to bug with essential matrix: #24482

modules/calib3d/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/calib3d/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/calib3d/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/calib3d/src/usac/ransac_solvers.cpp

@@ -733,7 +733,7 @@ public:
             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;
@@ -782,7 +782,7 @@ public:
                         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);
@@ -790,7 +790,8 @@ public:
                 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);
@@ -839,7 +840,7 @@ public:
                     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);
                     }