Merge pull request #24605 from MaximSmolskiy:speed-up-ChessBoardDetector-findQuadNeighbors

Speed up ChessBoardDetector::findQuadNeighbors #24605

### Pull Request Readiness Checklist

Replaced brute-force algorithm with O(N^2) time complexity with kd-tree with something like O(N * log N) time complexity (maybe only in average case).

For example, on image from #23558 without quads filtering (by using `CALIB_CB_FILTER_QUADS` flag) finding chessboards corners took ~770 seconds on my laptop, of which finding quads neighbors took ~620 seconds.

Now finding chessboards corners takes ~155-160 seconds, of which finding quads neighbors takes only ~5-10 seconds.

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
- [ ] There is accuracy test, performance test and test data in opencv_extra repository, if applicable
      Patch to opencv_extra has the same branch name.
- [ ] The feature is well documented and sample code can be built with the project CMake

modules/calib3d/src/calibinit.cpp

@@ -71,6 +71,7 @@
 
 #include "precomp.hpp"
 #include "circlesgrid.hpp"
+#include "opencv2/flann.hpp"
 
 #include <stack>
 
@@ -1588,7 +1589,24 @@ finalize:
 void ChessBoardDetector::findQuadNeighbors()
 {
     const float thresh_scale = 1.f;
+
+    const int all_corners_count = all_quads_count * 4;
+
+    std::vector<Point2f> all_quads_pts;
+    all_quads_pts.reserve(all_corners_count);
+    for (int idx = 0; idx < all_quads_count; idx++)
+    {
+        const ChessBoardQuad& cur_quad = (const ChessBoardQuad&)all_quads[idx];
+        for (int i = 0; i < 4; i++)
+            all_quads_pts.push_back(cur_quad.corners[i]->pt);
+    }
+
+    const cvflann::KDTreeSingleIndexParams index_params;
+    flann::GenericIndex<flann::L2_Simple<float>> all_quads_pts_index(Mat(all_quads_pts).reshape(1, all_corners_count), index_params);
+
     // find quad neighbors
+    std::vector<int> neighbors_indices(all_corners_count);
+    std::vector<float> neighbors_dists(all_corners_count);
     for (int idx = 0; idx < all_quads_count; idx++)
     {
         ChessBoardQuad& cur_quad = (ChessBoardQuad&)all_quads[idx];
@@ -1611,19 +1629,24 @@ void ChessBoardDetector::findQuadNeighbors()
             cv::Point2f pt = cur_quad.corners[i]->pt;
 
             // find the closest corner in all other quadrangles
-            for (int k = 0; k < all_quads_count; k++)
+            std::vector<float> query = Mat(pt);
+            float radius = cur_quad.edge_len * thresh_scale + 1;
+            const cvflann::SearchParams search_params(-1);
+            int neighbors_count = all_quads_pts_index.radiusSearch(query, neighbors_indices, neighbors_dists, radius, search_params);
+
+            for (int neighbor_idx_idx = 0; neighbor_idx_idx < neighbors_count; neighbor_idx_idx++)
             {
+                const int neighbor_idx = neighbors_indices[neighbor_idx_idx];
+                const int k = neighbor_idx >> 2;
                 if (k == idx)
                     continue;
 
                 ChessBoardQuad& q_k = all_quads[k];
-
-                for (int j = 0; j < 4; j++)
-                {
+                const int j = neighbor_idx & 3;
                 if (q_k.neighbors[j])
                     continue;
 
-                    float dist = normL2Sqr<float>(pt - q_k.corners[j]->pt);
+                const float dist = normL2Sqr<float>(pt - q_k.corners[j]->pt);
                 if (dist < min_dist &&
                     dist <= cur_quad.edge_len * thresh_scale &&
                     dist <= q_k.edge_len * thresh_scale)
@@ -1642,7 +1665,6 @@ void ChessBoardDetector::findQuadNeighbors()
                     min_dist = dist;
                 }
             }
-            }
 
             // we found a matching corner point?
             if (closest_corner_idx >= 0 && min_dist < FLT_MAX)
@@ -1681,15 +1703,21 @@ void ChessBoardDetector::findQuadNeighbors()
 
                 // check whether the closest corner to closest_corner
                 // is different from cur_quad->corners[i]->pt
-                for (j = 0; j < all_quads_count; j++ )
+                query = Mat(closest_corner.pt);
+                radius = min_dist + 1;
+                neighbors_count = all_quads_pts_index.radiusSearch(query, neighbors_indices, neighbors_dists, radius, search_params);
+
+                int neighbor_idx_idx = 0;
+                for (; neighbor_idx_idx < neighbors_count; neighbor_idx_idx++)
                 {
+                    const int neighbor_idx = neighbors_indices[neighbor_idx_idx];
+                    j = neighbor_idx >> 2;
+
                     ChessBoardQuad* q = &const_cast<ChessBoardQuad&>(all_quads[j]);
                     if (j == idx || q == closest_quad)
                         continue;
 
-                    int k = 0;
-                    for (; k < 4; k++ )
-                    {
+                    const int k = neighbor_idx & 3;
                     CV_DbgAssert(q);
                     if (!q->neighbors[k])
                     {
@@ -1697,10 +1725,7 @@ void ChessBoardDetector::findQuadNeighbors()
                             break;
                     }
                 }
-                    if (k < 4)
-                        break;
-                }
-                if (j < all_quads_count)
+                if (neighbor_idx_idx < neighbors_count)
                     continue;
 
                 closest_corner.pt = (pt + closest_corner.pt) * 0.5f;