I have a camera mounted on a gimbal, and I need to find the rvec
& tvec
between the camera and the gimbal.
So I rotate the gimbal, toked several picture of chessboard and record the corresponding IMU data (Euler angle) for each frame. But I can’t get the number to match the CAD scratch of the gimbal.
Here is the main part of my code.
 First I use
cv::calibrateCamera
to get thestd::vector<cv::Mat> tvec
&std::vector<cv::Mat> rvec
. (I get the error of0.488197
)
const auto error = calibrateCamera(
object_points,
image_points,
images[0].size(),
cameraMatrix,
distCoeffs,
rvecs,
tvecs);
 Then I convert my IMU data from Euler angle to rotation matrix by the following code, and push the result into
std::vector<cv::Mat> r_gripper2base
. (I calledinverse()
ofQuaternionf
here because I think the quaternion calculated from rotation might means “base2gripper”?)
const float cy = cos(yaw * 0.5f);
const float sy = sin(yaw * 0.5f);
const float cp = cos(pitch * 0.5f);
const float sp = sin(pitch * 0.5f);
const float cr = cos(roll * 0.5f);
const float sr = sin(roll * 0.5f);
Eigen::Quaternionf quaternion(
cy * cp * cr + sy * sp * sr,
cy * cp * sr  sy * sp * cr,
sy * cp * sr + cy * sp * cr,
sy * cp * cr  cy * sp * sr
);
cv::Mat rmat = quaternion.inverse().matrix();

Since I am ignoring the translation between base and gripper, so I let
t_gripper2base = cv::Mat::zeros(3, 1,CV_32F)
. 
Finally, I use the
cv::calibrateHandEye
to get the result.
calibrateHandEye(
r_gripper2base, // std::vector<cv::Mat> get from step 2
t_gripper2base, // std::vector<cv::Mat> get from step 3 (all zeros)
rvecs, // output from cv::calibrateCamera
tvecs, // output from cv::calibrateCamera
r_gripper2cam,
t_gripper2cam);
But the result I get is far off from the ground truth. And with further testing, I found that passing different value to the method parameter of cv::calibrateHandEye returns different result (a huge difference):
calibrateHandEye(
r_gripper2base,
t_gripper2base,
rvecs,
tvecs,
r_gripper2cam,
t_gripper2cam, // [3228.091028917876; 993.606961589237; 2952.829007343935]
cv::CALIB_HAND_EYE_PARK);
calibrateHandEye(
r_gripper2base,
t_gripper2base,
rvecs,
tvecs,
r_gripper2cam,
t_gripper2cam, // [255.7012843960561; 610.620177710132; 713.9109132572202]
cv::CALIB_HAND_EYE_TSAI);
I think I must have missed something but I don’t have the knowledge to aware what it is. Any suggestions would be appreciated!
Here is two example image I taken from camera (combined side by side).
And here is all the files to reproduce the result: Archive.zip