Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Speed up ChessBoardDetector::findQuadNeighbors #24605

Merged
merged 3 commits into from
Dec 11, 2023
Merged

Speed up ChessBoardDetector::findQuadNeighbors #24605

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
8a19d03
Speed up ChessBoardDetector::findQuadNeighbors
MaximSmolskiy Nov 28, 2023
8aeedb7
Fix
MaximSmolskiy Nov 28, 2023
370015e
Merge branch '4.x' into speed-up-ChessBoardDetector-findQuadNeighbors
MaximSmolskiy Nov 30, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
91 changes: 58 additions & 33 deletions modules/calib3d/src/calibinit.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -71,6 +71,7 @@

#include "precomp.hpp"
#include "circlesgrid.hpp"
#include "opencv2/flann.hpp"

#include <stack>

Expand Down Expand Up @@ -1588,7 +1589,24 @@ int ChessBoardDetector::checkQuadGroup(std::vector<ChessBoardQuad*>& quad_group,
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];
Expand All @@ -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<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);
Comment on lines +1633 to +1635
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@MaximSmolskiy, is there a case for which it will be necessary to increase the search radius? Is this possible only with large distortion?

Copy link
Contributor Author

@MaximSmolskiy MaximSmolskiy Dec 4, 2023 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@AleksandrPanov All these changes shouldn't change original behavior. I took this radius because below we have

                    if (dist < min_dist &&
                        dist <= cur_quad.edge_len*thresh_scale &&
                        dist <= q_k.edge_len*thresh_scale )

which means that we accept distance only if it less than cur_quad.edge_len * thresh_scale.
So, there is no sense in searching neighbors with radius greater than cur_quad.edge_len * thresh_scale - because we will never accept them further.
And I added + 1 as something like quite big "epsilon" to prevent possible problems with corner cases (e.g., if we want to search neighbors with radius <= R1 and flann module expects that if we pass radius R2 to it, then it searchs neighbors with condition radius < R2, then we should pass as R2 not exactly R1, but R1 + eps, where eps > 0 - this is just theoretical example, I don't know exactly in what area flann module searchs neighbors - radius < R2 or radius <= R2)


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<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)
{
if (q_k.neighbors[j])
continue;

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 )
// 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;
}
}

Expand Down Expand Up @@ -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<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])
{
CV_DbgAssert(q);
if (!q->neighbors[k])
{
if (normL2Sqr<float>(closest_corner.pt - q->corners[k]->pt) < min_dist)
break;
}
if (normL2Sqr<float>(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;
Expand Down