My goal is to find the position of a fixed camera near an ABB robot. To achieve this I use OpenCV with the calibrateHandEye() method.
My setup :
- A fixed camera relative to the robot base.
- An Aruco chessboard securely attached on the robot gripper.
What are the datas I use :
- The robot flange position in the base robot frame. For each point of view I get X,Y,Z in mm and Q0, Q1, Q2, Q3 (the quaternions for the orientation)
- The chessboard pose using
Cv2.SolvePnP()for each point of view.
My problem :
In the calibrateHandEye() method when using the HandEyeCalibrationMethod.PARK parameter the results look fine but with the HandEyeCalibrationMethod.TSAI parameters the results are totally wrong.
What is weird is that with the TSAI method the results are wrong only because of a specific point of view. If I remove the problematic point I get good results. I double checked this point and nothing looks wrong about it. The only particularity of this point is that Q3 does not have the same sign compare to the others.
What do I miss? Is there a concept I do not get about the HandEye calibration? Do I need to "convert" my quaternion when Q3 has a different sign?
The robot poses (X Y Z Q0 Q1 Q2 Q3):
752.38 -445.96 638.27 0.49314 0.82816 0.07749 -0.25488
695.07 -422.2 608.08 0.59008 0.55626 0.29763 -0.50378
676.35 -398.17 536.42 0.24081 0.79788 0.39938 -0.38195
564.8 -485.81 496.16 0.15692 0.67193 0.57801 -0.43565
636.3 -585.09 785.91 0.35118 0.70842 0.31395 -0.5256
674.15 -511.81 773.14 0.57508 0.58545 0.14986 -0.55143
571.73 -357.76 475.48 0.45984 0.76823 0.35026 -0.27512
570.54 -689.04 642.31 0.54275 0.73657 0.1737 -0.3643
522.64 -715.58 524.38 0.56756 0.69228 0.40363 -0.18898
959.89 -356.79 270.37 0.21973 0.84652 0.08542 -0.47732
797.93 -704.56 556.47 0.32647 0.79281 -0.20858 -0.47049
719.35 -976.8 364.49 0.30796 0.6863 -0.01742 -0.65867
906.75 -815.7 442.98 0.28293 0.61452 0.28696 -0.67821
817.77 60.02 154.15 0.39718 0.51765 0.5831 -0.48403
601.56 -569.27 480.03 0.73969 0.62158 0.24355 -0.08473
564.14 -408.19 612.04 0.43702 0.75943 0.42312 -0.23074
213.07 -606.02 670.48 0.43765 0.18263 0.87376 0.10798
The chessboard poses (X Y Z Rz Ry Rz):
-173.77009900744957 -27.139489979884726 721.9859325597129 -2.334719719539493 -0.6062024477154826 -0.28966237372137954
-162.65501847055066 -26.85185566224528 554.2767630115629 -3.0903465310356175 -0.2708982558600907 0.3291807930683886
-212.2479141822481 51.766662118465504 800.8059566577332 2.84556559822533 0.6680923622745605 1.0186319821149945
-86.09262298219434 126.5189807416448 819.6499137042784 2.4464190779627226 0.3475444809955279 0.9796705709434745
-21.898455311634194 89.93494845557872 450.972207473501 2.9168894650869452 0.7851694907456874 0.36096988692878934
-125.26779119062992 46.70400960098488 387.12034740949827 -2.890237270353615 -0.6482179109246602 0.49523655100783626
-206.47692825640706 101.15492953993694 853.5432013448462 -2.7845901323880615 -0.22557851509655572 -0.6048715250043196
95.71225425941525 150.04326736083527 632.9246651210359 -2.667387096309625 -0.49559063361443423 -0.0800873810963933
185.27210452976593 75.05506502382198 785.5205288697724 -2.736330698283884 0.2238771996238751 -0.47211319818633085
-377.8399022540909 -154.0271857570537 1048.9407456307115 -2.5031939821571854 -1.4073251338872734 -0.564144060449975
-97.9813803618571 -2.8982716572417337 780.3826436567616 -2.0175024432723028 -1.52111122787833 0.07978790180677371
206.26218058582174 76.52368830230974 839.4288605610492 -2.5926961439942273 -1.6274800425133538 0.23606702098023685
168.593405541405 -172.2903924334086 728.3609797990065 2.5826463269087228 0.9741779880929422 0.06773704115655899
-599.5692170589801 -208.7756878651145 1082.1873391173815 2.643714241537085 -0.05662133314429313 0.32661484396025725
42.774011707655745 -13.244986693290826 777.4026418514148 -2.3533206372842397 0.38523715542666004 0.0006794851359404635
-146.04506073796117 71.8778078758312 735.3038919876881 -2.7999700612696636 -0.05617005954652475 -0.7930410515888396
-93.2419492180757 102.4051550208101 689.1444785949833 1.8674927518469928 -1.9023073605673702 0.8138748033587386
My results : Translation X,Y,Z of my camera relative to the robot : 543.69 -551.87 282.19 (totally wrong results)
With same datas except the last one : 518.87 -769.82 1273.86 (coherent results)
My code if it can help : (I use OpenCV in C# with the nugget OpenCvSharp but you can provide me python or C++ code)
- To convert quaternion to rotation matrix :
private double[,] Quaternion2RotationMatrix(float q0, float q1, float q2, float q3)
{
double[,] matrix = new double[3, 3];
matrix[0, 0] = 2 * (q0 * q0 + q1 * q1) - 1;
matrix[0, 1] = 2 * (q1 * q2 - q0 * q3);
matrix[0, 2] = 2 * (q1 * q3 + q0 * q2);
matrix[1, 0] = 2 * (q1 * q2 + q0 * q3);
matrix[1, 1] = 2 * (q0 * q0 + q2 * q2) - 1;
matrix[1, 2] = 2 * (q2 * q3 - q0 * q1);
matrix[2, 0] = 2 * (q1 * q3 - q0 * q2);
matrix[2, 1] = 2 * (q2 * q3 + q0 * q1);
matrix[2, 2] = 2 * (q0 * q0 + q3 * q3) - 1;
return matrix;
}
- To convert a Mat object to a Array
private double[,] ConvertMat2Array(Mat _mat, bool _isFloat = true)
{
double[,] result = new double[_mat.Rows, _mat.Cols];
for (int i = 0; i < _mat.Rows; i++)
{
for (int j = 0; j < _mat.Cols; j++)
{
if (_isFloat)
result[i, j] = _mat.At<float>(i, j);
else
result[i, j] = _mat.At<double>(i, j);
}
}
return result;
}
- My Pose3D class to store the robot coordinates
public class Pose3D
{
public float X { get; set; }
public float Y { get; set; }
public float Z { get; set; }
public float Q0 { get; set; }
public float Q1 { get; set; }
public float Q2 { get; set; }
public float Q3 { get; set; }
public Pose3D()
{
}
public Pose3D(float _x, float _y, float _z, float _q0, float _q1, float _q2, float _q3) : this()
{
X = _x;
Y = _y;
Z = _z;
Q0 = _q0;
Q1 = _q1;
Q2 = _q2;
Q3 = _q3;
}
}
- And finally my method to estimate the camera position in the robot base frame :
private (double[,], double[,]) HandEyeCalibration(Pose3D[] _gripper2base, double[][] _rTarget2cam, double[][] _tTarget2Cam)
{
try
{
var l = _gripper2base.Length;
Mat[] R_base2gripper = new Mat[l];
Mat[] t_base2gripper = new Mat[l];
Mat[] R_target2cam = new Mat[l];
Mat[] t_target2cam = new Mat[l];
for (int i = 0; i < l; i++)
{
// Convert quaternion to rotation matrix
var R_gripper2base = Quaternion2RotationMatrix(_gripper2base[i].Q0, _gripper2base[i].Q1, _gripper2base[i].Q2, _gripper2base[i].Q3);
double[,] T_gripper2base = new double[4, 4];
for (int row = 0; row < 3; row++)
for (int col = 0; col < 3; col++)
T_gripper2base[row, col] = R_gripper2base[row, col];
T_gripper2base[0, 3] = _gripper2base[i].X;
T_gripper2base[1, 3] = _gripper2base[i].Y;
T_gripper2base[2, 3] = _gripper2base[i].Z;
T_gripper2base[3, 3] = 1;
// Eye to Hand configuration. Need to invert gripper2base to get base2gripper to find cam2base
Mat mat = new Mat(4, 4, MatType.CV_64FC1, T_gripper2base);
var invExpr = mat.Inv();
var inv = invExpr.ToMat();
// Extract rotation matrix and translation vector
R_base2gripper[i] = inv.SubMat(0, 3, 0, 3);
t_base2gripper[i] = inv.SubMat(0, 3, 3, 4);
Mat tmp_R_t2c = new Mat(3, 1, MatType.CV_64FC1, _rTarget2cam[i]);
Mat tmp_t_t2c = new Mat(3, 1, MatType.CV_64FC1, _tTarget2Cam[i]);
R_target2cam[i] = tmp_R_t2c;
t_target2cam[i] = tmp_t_t2c;
}
using Mat R_cam2base = new Mat();
using Mat t_cam2base = new Mat();
// Replace HandEyeCalibrationMethod.TSAI by HandEyeCalibrationMethod.PARK to change the method used.
Cv2.CalibrateHandEye(R_base2gripper, t_base2gripper, R_target2cam, t_target2cam, R_cam2base, t_cam2base, HandEyeCalibrationMethod.TSAI);
var rotationMatrix = ConvertMat2Array(R_cam2base, false);
var translationMatrix = ConvertMat2Array(t_cam2base, false);
return (rotationMatrix, translationMatrix);
}
catch (Exception ex)
{
return (null, null);
}
}
Thank you !