Fix handeye #24897
There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -289,32 +289,55 @@ 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++) | ||
| { | ||
| //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 | ||
| //Rotation axis for Rgij which is the 3D rotation from gripper coordinate frame Gi to Gj | ||
| Mat Pgij = 2*rot2quatMinimal(Hgij); | ||
|
|
||
| //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 = Hc[j] * homogeneousInverse(Hc[i]); //eq 7 | ||
| //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) { | ||
JStech marked this conversation as resolved.
Show resolved
Hide resolved
|
||
| 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) | ||
|
|
@@ -327,28 +350,24 @@ static void calibrateHandEyeTsai(const std::vector<Mat>& Hg, const std::vector<M | |
|
|
||
| Mat Rcg = quatMinimal2rot(Pcg/2.0); | ||
|
|
||
| for (size_t i = 0; i < vec_Hgij.size(); i++) | ||
| { | ||
| //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, 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, static_cast<int>(i)*3, 1, 3))); | ||
| } | ||
|
|
||
| Mat Tcg; | ||
|
|
@@ -449,6 +468,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; | ||
|
Contributor
There was a problem hiding this comment. Indeed, this can be checked with this online tool: https://www.andre-gaschler.com/rotationconverter/ |
||
| } | ||
| double r0 = qgij.at<double>(0,0); | ||
| double rx = qgij.at<double>(1,0); | ||
| double ry = qgij.at<double>(2,0); | ||
|
|
@@ -461,6 +483,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); | ||
|
|
@@ -618,7 +643,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)); | ||
|
|
||
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Makes sense.