From dbdd357b0af0778045b32a5b23275d6b77093a0f Mon Sep 17 00:00:00 2001
From: Funatomi Takuya <funatomi@is.naist.jp>
Date: Tue, 29 Nov 2022 13:08:40 +0000
Subject: [PATCH] Extend USAC coverage.

Add `estimateSE2(...)`, `estimateSE3(...)`, `estimateSIM2(...)`, `estimateSIM3(...)` for estimating an geometric transformation with rotation and translation (with scaling for SIM) using USAC: as alternative for `estimateAffinePartial2D` and `estimateAffine3D`.

Modified test module.

Remove unused variables.

Remove initializer of unused variable.

Add interfaces to accept UsacParams() and corresponding test codes.

Revise test code.

PartialNd removed

Umeyama rewritten for code quality & speed

comments & minors

rise number of points

fix, and +30% faster!

only one number should be that big

remove USAC code, leave fix only

big number
---
 modules/calib3d/src/precomp.hpp                 |  2 ++
 modules/calib3d/src/usac/local_optimization.cpp | 14 +++++++-------
 modules/calib3d/test/test_usac.cpp              |  2 +-
 3 files changed, 10 insertions(+), 8 deletions(-)

diff --git a/modules/calib3d/src/precomp.hpp b/modules/calib3d/src/precomp.hpp
index f2cb7123c0..610deebfa9 100644
--- a/modules/calib3d/src/precomp.hpp
+++ b/modules/calib3d/src/precomp.hpp
@@ -53,6 +53,8 @@
 
 #include "opencv2/core/ocl.hpp"
 
+#include <set>
+
 #define GET_OPTIMIZED(func) (func)
 
 
diff --git a/modules/calib3d/src/usac/local_optimization.cpp b/modules/calib3d/src/usac/local_optimization.cpp
index ff8089bb8f..3964edec9a 100644
--- a/modules/calib3d/src/usac/local_optimization.cpp
+++ b/modules/calib3d/src/usac/local_optimization.cpp
@@ -20,7 +20,7 @@ protected:
 
     std::vector<int> labeling_inliers;
     std::vector<double> energies, weights;
-    std::vector<bool> used_edges;
+    std::set<int> used_edges;
     std::vector<Mat> gc_models;
 public:
 
@@ -40,7 +40,7 @@ public:
 
         energies = std::vector<double>(points_size);
         labeling_inliers = std::vector<int>(points_size);
-        used_edges = std::vector<bool>(points_size*points_size);
+        used_edges = std::set<int>();
         gc_models = std::vector<Mat> (estimator->getMaxNumSolutionsNonMinimal());
     }
 
@@ -115,7 +115,7 @@ private:
             energies[pt] = energy > 1 ? 1 : energy;
         }
 
-        std::fill(used_edges.begin(), used_edges.end(), false);
+        used_edges.clear();
 
         bool has_edges = false;
         // Iterate through all points and set their edges
@@ -125,12 +125,12 @@ private:
             // Iterate through  all neighbors
             for (int actual_neighbor_idx : neighborhood_graph->getNeighbors(point_idx)) {
                 if (actual_neighbor_idx == point_idx ||
-                    used_edges[actual_neighbor_idx*points_size + point_idx] ||
-                    used_edges[point_idx*points_size + actual_neighbor_idx])
+                    used_edges.count(actual_neighbor_idx*points_size + point_idx) > 0 ||
+                    used_edges.count(point_idx*points_size + actual_neighbor_idx) > 0)
                     continue;
 
-                used_edges[actual_neighbor_idx*points_size + point_idx] = true;
-                used_edges[point_idx*points_size + actual_neighbor_idx] = true;
+                used_edges.insert(actual_neighbor_idx*points_size + point_idx);
+                used_edges.insert(point_idx*points_size + actual_neighbor_idx);
 
                 double a = (0.5 * (energy + energies[actual_neighbor_idx])) * spatial_coherence,
                        b = spatial_coherence, c = spatial_coherence, d = 0;
diff --git a/modules/calib3d/test/test_usac.cpp b/modules/calib3d/test/test_usac.cpp
index fc36e8c945..f7c4b9e4ce 100644
--- a/modules/calib3d/test/test_usac.cpp
+++ b/modules/calib3d/test/test_usac.cpp
@@ -416,7 +416,7 @@ TEST (usac_Affine2D, accuracy) {
 
 TEST(usac_testUsacParams, accuracy) {
     std::vector<int> gt_inliers;
-    const int pts_size = 1500;
+    const int pts_size = 150000;
     cv::RNG &rng = cv::theRNG();
     const cv::UsacParams usac_params = cv::UsacParams();
     cv::Mat pts1, pts2, K1, K2, mask, model, rvec, tvec, R;