potok/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2026-07-31 00:03:03 +04:00
Code Packages Releases Wiki Activity

Merge branch '4.x' into '5.x'

Browse Source
This commit is contained in:
Maksim Shabunin
2024-06-11 19:38:59 +03:00
parent a2d2ea6536 a03b813167
commit 26ea34c4cb
573 changed files with 72922 additions and 7355 deletions
Download Patch File Download Diff File Expand all files Collapse all files
modules/calib/src/calibinit.cpp
+184 -123
View File
@@ -220,6 +220,26 @@ public:
int all_quads_count;
struct NeighborsFinder {
const float thresh_scale = 1.f;
ChessBoardDetector& detector;
std::vector<int> neighbors_indices;
std::vector<float> neighbors_dists;
std::vector<Point2f> all_quads_pts;
flann::GenericIndex<flann::L2_Simple<float>> all_quads_pts_index;
NeighborsFinder(ChessBoardDetector& detector);
bool findCornerNeighbor(
const int idx,
const cv::Point2f& pt,
float& min_dist,
const float radius,
int& closest_quad_idx,
int& closest_corner_idx,
cv::Point2f& closest_corner_pt);
};
ChessBoardDetector(const Size& pattern_size_) :
pattern_size(pattern_size_),
all_quads_count(0)
@@ -470,6 +490,125 @@ static void icvBinarizationHistogramBased(Mat & img)
}
}
static std::vector<Point2f> getCornersFromQuads(ChessBoardQuad* p_all_quads, const int all_quads_count)
{
std::vector<Point2f> all_quads_pts;
all_quads_pts.reserve(all_quads_count * 4);
for (int idx = 0; idx < all_quads_count; idx++)
{
const ChessBoardQuad& cur_quad = (const ChessBoardQuad&)p_all_quads[idx];
for (int i = 0; i < 4; i++)
all_quads_pts.push_back(cur_quad.corners[i]->pt);
}
return all_quads_pts;
}
ChessBoardDetector::NeighborsFinder::NeighborsFinder(ChessBoardDetector& _detector) :
detector(_detector),
all_quads_pts(getCornersFromQuads(detector.all_quads.data(), detector.all_quads_count)),
all_quads_pts_index(Mat(all_quads_pts).reshape(1, detector.all_quads_count * 4), cvflann::KDTreeSingleIndexParams())
{
const int all_corners_count = detector.all_quads_count * 4;
neighbors_indices.resize(all_corners_count);
neighbors_dists.resize(all_corners_count);
}
bool ChessBoardDetector::NeighborsFinder::findCornerNeighbor(
const int idx,
const cv::Point2f& pt,
float& min_dist,
const float radius,
int& closest_quad_idx,
int& closest_corner_idx,
cv::Point2f& closest_corner_pt)
{
ChessBoardQuad* p_all_quads = detector.all_quads.data();
const ChessBoardQuad& cur_quad = (const ChessBoardQuad&)p_all_quads[idx];
int closest_neighbor_idx = -1;
ChessBoardQuad *closest_quad = 0;
// find the closest corner in all other quadrangles
const std::vector<float> query = { pt.x, pt.y };
const cvflann::SearchParams search_params(-1);
const 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 = p_all_quads[k];
const int j = neighbor_idx & 3;
if (q_k.neighbors[j])
continue;
const float dist = normL2Sqr<float>(pt - all_quads_pts[neighbor_idx]);
if (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.
// edge_len is squared edge length, so we compare them
// with squared constant 16 = 4^2
if (q_k.edge_len > 16 * cur_quad.edge_len ||
cur_quad.edge_len > 16 * q_k.edge_len)
{
DPRINTF("Incompatible edge lengths");
continue;
}
closest_neighbor_idx = neighbor_idx;
closest_quad_idx = k;
closest_corner_idx = j;
closest_quad = &q_k;
min_dist = dist;
break;
}
}
// we found a matching corner point?
if (closest_neighbor_idx >= 0 && closest_quad_idx >= 0 && closest_corner_idx >= 0 && min_dist < FLT_MAX)
{
CV_Assert(closest_quad);
if (cur_quad.count >= 4 || closest_quad->count >= 4)
return false;
// If another point from our current quad is closer to the found corner
// than the current one, then we don't count this one after all.
// This is necessary to support small squares where otherwise the wrong
// corner will get matched to closest_quad;
closest_corner_pt = all_quads_pts[closest_neighbor_idx];
int j = 0;
for (; j < 4; j++)
{
if (cur_quad.neighbors[j] == closest_quad)
break;
if (normL2Sqr<float>(closest_corner_pt - all_quads_pts[(idx << 2) + j]) < min_dist)
break;
}
if (j < 4)
return false;
// Check that each corner is a neighbor of different quads
for(j = 0; j < 4; j++ )
{
if (closest_quad->neighbors[j] == &cur_quad)
break;
}
if (j < 4)
return false;
return true;
}
return false;
}
bool findChessboardCorners(InputArray image_, Size pattern_size,
OutputArray corners_, int flags)
{
@@ -1606,25 +1745,7 @@ 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);
NeighborsFinder neighborsFinder(*this);
for (int idx = 0; idx < all_quads_count; idx++)
{
ChessBoardQuad& cur_quad = (ChessBoardQuad&)all_quads[idx];
@@ -1640,125 +1761,65 @@ void ChessBoardDetector::findQuadNeighbors()
if (cur_quad.neighbors[i])
continue;
const cv::Point2f pt = neighborsFinder.all_quads_pts[(idx << 2) + i];
float min_dist = FLT_MAX;
int closest_neighbor_idx = -1;
int closest_quad_idx = -1;
int closest_corner_idx = -1;
ChessBoardQuad *closest_quad = 0;
Point2f pt = all_quads_pts[(idx << 2) + i];
float radius = cur_quad.edge_len * neighborsFinder.thresh_scale + 1;
// find the closest corner in all other quadrangles
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);
cv::Point2f closest_corner_pt;
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;
bool found = neighborsFinder.findCornerNeighbor(
idx,
pt,
min_dist,
radius,
closest_quad_idx,
closest_corner_idx,
closest_corner_pt);
ChessBoardQuad& q_k = all_quads[k];
const int j = neighbor_idx & 3;
if (q_k.neighbors[j])
continue;
if (!found)
continue;
const float dist = normL2Sqr<float>(pt - all_quads_pts[neighbor_idx]);
if (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.
// edge_len is squared edge length, so we compare them
// with squared constant 16 = 4^2
if (q_k.edge_len > 16 * cur_quad.edge_len ||
cur_quad.edge_len > 16 * q_k.edge_len)
{
DPRINTF("Incompatible edge lengths");
continue;
}
closest_neighbor_idx = neighbor_idx;
closest_corner_idx = j;
closest_quad = &q_k;
min_dist = dist;
break;
}
}
radius = min_dist + 1;
min_dist = FLT_MAX;
// we found a matching corner point?
if (closest_neighbor_idx >= 0 && closest_corner_idx >= 0 && min_dist < FLT_MAX)
{
CV_Assert(closest_quad);
int closest_closest_quad_idx = -1;
int closest_closest_corner_idx = -1;
if (cur_quad.count >= 4 || closest_quad->count >= 4)
continue;
cv::Point2f closest_closest_corner_pt;
// If another point from our current quad is closer to the found corner
// than the current one, then we don't count this one after all.
// This is necessary to support small squares where otherwise the wrong
// corner will get matched to closest_quad;
ChessBoardCorner& closest_corner = *closest_quad->corners[closest_corner_idx];
cv::Point2f closest_corner_pt = all_quads_pts[closest_neighbor_idx];
found = neighborsFinder.findCornerNeighbor(
closest_quad_idx,
closest_corner_pt,
min_dist,
radius,
closest_closest_quad_idx,
closest_closest_corner_idx,
closest_closest_corner_pt);
int j = 0;
for (; j < 4; j++)
{
if (cur_quad.neighbors[j] == closest_quad)
break;
if (!found)
continue;
if (normL2Sqr<float>(closest_corner_pt - all_quads_pts[(idx << 2) + j]) < min_dist)
break;
}
if (j < 4)
continue;
if (closest_closest_quad_idx != idx ||
closest_closest_corner_idx != i ||
closest_closest_corner_pt != pt)
continue;
// Check that each corner is a neighbor of different quads
for(j = 0; j < 4; j++ )
{
if (closest_quad->neighbors[j] == &cur_quad)
break;
}
if (j < 4)
continue;
ChessBoardQuad* closest_quad = &all_quads[closest_quad_idx];
ChessBoardCorner& closest_corner = *closest_quad->corners[closest_corner_idx];
closest_corner.pt = (pt + closest_corner_pt) * 0.5f;
// check whether the closest corner to closest_corner is different from pt
query = Mat(closest_corner_pt);
radius = min_dist + 1;
neighbors_count = all_quads_pts_index.radiusSearch(query, neighbors_indices, neighbors_dists, radius, search_params);
// We've found one more corner - remember it
cur_quad.count++;
cur_quad.neighbors[i] = closest_quad;
cur_quad.corners[i] = &closest_corner;
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;
const int k = neighbor_idx & 3;
CV_DbgAssert(q);
if (!q->neighbors[k])
{
if (normL2Sqr<float>(closest_corner_pt - all_quads_pts[neighbor_idx]) < min_dist)
break;
}
}
if (neighbor_idx_idx < neighbors_count)
continue;
closest_corner.pt = (pt + closest_corner_pt) * 0.5f;
// We've found one more corner - remember it
cur_quad.count++;
cur_quad.neighbors[i] = closest_quad;
cur_quad.corners[i] = &closest_corner;
closest_quad->count++;
closest_quad->neighbors[closest_corner_idx] = &cur_quad;
}
closest_quad->count++;
closest_quad->neighbors[closest_corner_idx] = &cur_quad;
}
}
}
modules/calib/src/calibration.cpp
+1 -1
View File
@@ -1863,7 +1863,7 @@ double calibrateCameraRO(InputArrayOfArrays _objectPoints,
rvecM, tvecM,
newObjPt,
stdDeviationsM,
errorsM, flags, cvTermCriteria(criteria));
errorsM, flags, criteria);
if( stddev_needed )
{
modules/calib/src/calibration_handeye.cpp
+69 -26
View File
@@ -288,13 +288,12 @@ static void calibrateHandEyeTsai(const std::vector<Mat>& Hg, const std::vector<M
int idx = 0;
for (size_t i = 0; i < Hg.size(); i++)
{
for (size_t j = i+1; j < Hg.size(); j++, idx++)
for (size_t j = i+1; j < Hg.size(); j++)
{
//Defines coordinate transformation from Gi to Gj
//Hgi is from Gi (gripper) to RW (robot base)
//Hgj is from Gj (gripper) to RW (robot base)
Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i]; //eq 6
vec_Hgij.push_back(Hgij);
//Rotation axis for Rgij which is the 3D rotation from gripper coordinate frame Gi to Gj
Mat Pgij = 2*rot2quatMinimal(Hgij);
@@ -302,18 +301,42 @@ static void calibrateHandEyeTsai(const std::vector<Mat>& Hg, const std::vector<M
//Hci is from CW (calibration target) to Ci (camera)
//Hcj is from CW (calibration target) to Cj (camera)
Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]); //eq 7
vec_Hcij.push_back(Hcij);
//Rotation axis for Rcij
Mat Pcij = 2*rot2quatMinimal(Hcij);
// Discard motions with rotation too small or too close to pi radians
// The limits 1.7 and 0.3 correspond to angles less than 17 degrees or greater than 120 degrees. They are
// based on verifying equation 12 from the source paper using data generated with a known hand-eye
// calibration. The data contained 25 poses, so 300 motions were considered. Of these, 188 satisfied
// equation 12, and the remaining 112 all had Pcij or Pgij with norms greater than 1.7. Although errors
// from small rotations were not observed, it is known that these motions are less informative (see
// section II.B.3, and figure 6).
double Pgij_norm = cv::norm(Pgij);
double Pcij_norm = cv::norm(Pcij);
if (Pgij_norm < 0.3 || Pcij_norm < 0.3 || Pgij_norm > 1.7 || Pcij_norm > 1.7) {
continue;
}
vec_Hgij.push_back(Hgij);
vec_Hcij.push_back(Hcij);
//Left-hand side: skew(Pgij+Pcij)
skew(Pgij+Pcij).copyTo(A(Rect(0, idx*3, 3, 3)));
//Right-hand side: Pcij - Pgij
Mat diff = Pcij - Pgij;
diff.copyTo(B(Rect(0, idx*3, 1, 3)));
idx++;
}
}
// insufficient data
if (idx < 2) {
CV_LOG_ERROR(NULL, "Hand-eye calibration failed! Not enough informative motions--include larger rotations.");
return;
}
A.resize(3*idx);
B.resize(3*idx);
Mat Pcg_;
//Rotation from camera to gripper is obtained from the set of equations:
// skew(Pgij+Pcij) * Pcg_ = Pcij - Pgij (eq 12)
@@ -326,28 +349,24 @@ static void calibrateHandEyeTsai(const std::vector<Mat>& Hg, const std::vector<M
Mat Rcg = quatMinimal2rot(Pcg/2.0);
idx = 0;
for (size_t i = 0; i < Hg.size(); i++)
for (size_t i = 0; i < vec_Hgij.size(); i++)
{
for (size_t j = i+1; j < Hg.size(); j++, idx++)
{
//Defines coordinate transformation from Gi to Gj
//Hgi is from Gi (gripper) to RW (robot base)
//Hgj is from Gj (gripper) to RW (robot base)
Mat Hgij = vec_Hgij[static_cast<size_t>(idx)];
//Defines coordinate transformation from Ci to Cj
//Hci is from CW (calibration target) to Ci (camera)
//Hcj is from CW (calibration target) to Cj (camera)
Mat Hcij = vec_Hcij[static_cast<size_t>(idx)];
//Defines coordinate transformation from Gi to Gj
//Hgi is from Gi (gripper) to RW (robot base)
//Hgj is from Gj (gripper) to RW (robot base)
Mat Hgij = vec_Hgij[i];
//Defines coordinate transformation from Ci to Cj
//Hci is from CW (calibration target) to Ci (camera)
//Hcj is from CW (calibration target) to Cj (camera)
Mat Hcij = vec_Hcij[i];
//Left-hand side: (Rgij - I)
Mat diff = Hgij(Rect(0,0,3,3)) - Mat::eye(3,3,CV_64FC1);
diff.copyTo(A(Rect(0, idx*3, 3, 3)));
//Left-hand side: (Rgij - I)
Mat diff = Hgij(Rect(0,0,3,3)) - Mat::eye(3,3,CV_64FC1);
diff.copyTo(A(Rect(0, static_cast<int>(i)*3, 3, 3)));
//Right-hand side: Rcg*Tcij - Tgij
diff = Rcg*Hcij(Rect(3, 0, 1, 3)) - Hgij(Rect(3, 0, 1, 3));
diff.copyTo(B(Rect(0, idx*3, 1, 3)));
}
//Right-hand side: Rcg*Tcij - Tgij
diff = Rcg*Hcij(Rect(3, 0, 1, 3)) - Hgij(Rect(3, 0, 1, 3));
diff.copyTo(B(Rect(0, static_cast<int>(i)*3, 1, 3)));
}
Mat Tcg;
@@ -448,6 +467,9 @@ static void calibrateHandEyeHoraud(const std::vector<Mat>& Hg, const std::vector
Mat Rcij = Hcij(Rect(0, 0, 3, 3));
Mat qgij = rot2quat(Rgij);
if (qgij.at<double>(0, 0) < 0) {
qgij *= -1;
}
double r0 = qgij.at<double>(0,0);
double rx = qgij.at<double>(1,0);
double ry = qgij.at<double>(2,0);
@@ -460,6 +482,9 @@ static void calibrateHandEyeHoraud(const std::vector<Mat>& Hg, const std::vector
rz, -ry, rx, r0);
Mat qcij = rot2quat(Rcij);
if (qcij.at<double>(0, 0) < 0) {
qcij *= -1;
}
r0 = qcij.at<double>(0,0);
rx = qcij.at<double>(1,0);
ry = qcij.at<double>(2,0);
@@ -617,7 +642,13 @@ static void calibrateHandEyeDaniilidis(const std::vector<Mat>& Hg, const std::ve
Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]);
Mat dualqa = homogeneous2dualQuaternion(Hgij);
if (dualqa.at<double>(0, 0) < 0) {
dualqa *= -1;
}
Mat dualqb = homogeneous2dualQuaternion(Hcij);
if (dualqb.at<double>(0, 0) < 0) {
dualqb *= -1;
}
Mat a = dualqa(Rect(0, 1, 1, 3));
Mat b = dualqb(Rect(0, 1, 1, 3));
@@ -721,7 +752,11 @@ void calibrateHandEye(InputArrayOfArrays R_gripper2base, InputArrayOfArrays t_gr
if(R_gripper2base_[i].size() == Size(3, 3))
R_gripper2base_[i].convertTo(R, CV_64F);
else
Rodrigues(R_gripper2base_[i], R);
{
cv::Mat R_temp;
Rodrigues(R_gripper2base_[i], R_temp);
R_temp.convertTo(R, CV_64F);
}
Mat t = m(Rect(3, 0, 1, 3));
t_gripper2base_[i].convertTo(t, CV_64F);
@@ -739,7 +774,11 @@ void calibrateHandEye(InputArrayOfArrays R_gripper2base, InputArrayOfArrays t_gr
if(R_target2cam_[i].size() == Size(3, 3))
R_target2cam_[i].convertTo(R, CV_64F);
else
Rodrigues(R_target2cam_[i], R);
{
cv::Mat R_temp;
Rodrigues(R_target2cam_[i], R_temp);
R_temp.convertTo(R, CV_64F);
}
Mat t = m(Rect(3, 0, 1, 3));
t_target2cam_[i].convertTo(t, CV_64F);
@@ -919,7 +958,9 @@ void calibrateRobotWorldHandEye(InputArrayOfArrays R_world2cam, InputArrayOfArra
}
else
{
Rodrigues(rot, R);
cv::Mat R_temp;
Rodrigues(rot, R_temp);
R_temp.convertTo(R, CV_64F);
R_base2gripper_.push_back(R);
}
Mat tvec = t_base2gripper_tmp[i];
@@ -937,7 +978,9 @@ void calibrateRobotWorldHandEye(InputArrayOfArrays R_world2cam, InputArrayOfArra
}
else
{
Rodrigues(rot, R);
cv::Mat R_temp;
Rodrigues(rot, R_temp);
R_temp.convertTo(R, CV_64F);
R_world2cam_.push_back(R);
}
Mat tvec = t_world2cam_tmp[i];