From b1b59c87b93e7964868f51eff72b9e3d18ef9795 Mon Sep 17 00:00:00 2001 From: Maxim Smolskiy Date: Mon, 11 Dec 2023 19:11:58 +0300 Subject: [PATCH] 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 | 91 ++++++++++++++++++++----------- 1 file changed, 58 insertions(+), 33 deletions(-) diff --git a/modules/calib3d/src/calibinit.cpp b/modules/calib3d/src/calibinit.cpp index e4909bb575..9693eaf0d4 100644 --- a/modules/calib3d/src/calibinit.cpp +++ b/modules/calib3d/src/calibinit.cpp @@ -71,6 +71,7 @@ #include "precomp.hpp" #include "circlesgrid.hpp" +#include "opencv2/flann.hpp" #include @@ -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 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> all_quads_pts_index(Mat(all_quads_pts).reshape(1, all_corners_count), index_params); + // find quad neighbors + std::vector neighbors_indices(all_corners_count); + std::vector neighbors_dists(all_corners_count); for (int idx = 0; idx < all_quads_count; idx++) { ChessBoardQuad& cur_quad = (ChessBoardQuad&)all_quads[idx]; @@ -1611,36 +1629,40 @@ 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 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]; + const int j = neighbor_idx & 3; + if (q_k.neighbors[j]) + continue; - for (int j = 0; j < 4; j++) + const float dist = normL2Sqr(pt - q_k.corners[j]->pt); + if (dist < min_dist && + dist <= cur_quad.edge_len * thresh_scale && + dist <= q_k.edge_len * thresh_scale) { - if (q_k.neighbors[j]) - continue; - - float dist = normL2Sqr(pt - q_k.corners[j]->pt); - if (dist < min_dist && - dist <= cur_quad.edge_len*thresh_scale && - dist <= q_k.edge_len*thresh_scale ) + // check edge lengths, make sure they're compatible + // edges that are different by more than 1:4 are rejected + const float ediff = fabs(cur_quad.edge_len - q_k.edge_len); + if (ediff > 32 * cur_quad.edge_len || + ediff > 32 * q_k.edge_len) { - // check edge lengths, make sure they're compatible - // edges that are different by more than 1:4 are rejected - const float ediff = fabs(cur_quad.edge_len - q_k.edge_len); - if (ediff > 32*cur_quad.edge_len || - ediff > 32*q_k.edge_len) - { - DPRINTF("Incompatible edge lengths"); - continue; - } - closest_corner_idx = j; - closest_quad = &q_k; - min_dist = dist; + DPRINTF("Incompatible edge lengths"); + continue; } + closest_corner_idx = j; + closest_quad = &q_k; + min_dist = dist; } } @@ -1681,26 +1703,29 @@ 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(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]) { - CV_DbgAssert(q); - if (!q->neighbors[k]) - { - if (normL2Sqr(closest_corner.pt - q->corners[k]->pt) < min_dist) - break; - } + if (normL2Sqr(closest_corner.pt - q->corners[k]->pt) < min_dist) + 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;