How to find robot TCP in image with CalibrateHandEye - c++

Question

I'm currently developing a programm for a robot with a camera attached to the end affector.
The goal is to calculate where the robots TCP appears in the camera frame. I'm using opencv in c++.
The robot is a UR5 from Universal Robots.
My plan:
collect multiple (8) data-sets:
robot pose (XYZ in meters, directly from the robot controller)
robot rotation (rx ry rz in radians, directly from the robot controller)
take a picture of calibration pattern for each step
run calibrateCamera over the set of pictures to get tvec and rvec for every step
run calibrateHandEye
for t_gripper2base i use the robot pose
for R_gripper2base i use the robot rotation
for t_target2cam i use tvec from calibrateCamera
for R_target2cam i use rvec from calibrateCamera
I seem to get correct values (I measured the distance from cam to TCP and the t_cam2gripper seems to be correct.
Translation vector target to cam:
[-0.0001052803107026547;
-0.0780872727019615;
-0.1243323507069755]
Rotation matrix target to cam:
[0.9999922523048892, 0.002655868335207422, -0.002905459271957709;
-0.001229768871633805, 0.9119334002787367, 0.4103363999508009;
0.003739384804660116, -0.4103296477461107, 0.9119296010009958]
The formula to transform the point from TCP coordinates to the image should look like this:
(u,v,w) = C * T * (X,Y,Z,1)
But after the division by w my values are still way off (should be around (600,600)
Actual image position vector homogenized:
[1778.542462313536;
-1626.72483032188;
1]
#include <QCoreApplication>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/opencv.hpp>
using namespace std;
using namespace cv;
int main(int argc, char *argv[])
{
QCoreApplication a(argc, argv);
Mat defaultCameraMatrix = (Mat_<double>(3, 3));
Mat defaultDistortion = (Mat_<double>(1, 5));
defaultCameraMatrix.at<double>(0, 0) = 1739.3749; // default values from previous intrinsic camera calibration
defaultCameraMatrix.at<double>(0, 1) = 0;
defaultCameraMatrix.at<double>(0, 2) = 639.5;
defaultCameraMatrix.at<double>(1, 0) = 0;
defaultCameraMatrix.at<double>(1, 1) = 1739.3749;
defaultCameraMatrix.at<double>(1, 2) = 479.5;
defaultCameraMatrix.at<double>(2, 0) = 0;
defaultCameraMatrix.at<double>(2, 1) = 0;
defaultCameraMatrix.at<double>(2, 2) = 1;
defaultDistortion.at<double>(0, 0) = -0.165909;
defaultDistortion.at<double>(0, 1) = 0.303675;
defaultDistortion.at<double>(0, 2) = 0.0;
defaultDistortion.at<double>(0, 3) = 0.0;
defaultDistortion.at<double>(0, 4) = 0.0;
vector<Mat> R_gripper2base, t_gripper2base, R_target2cam, t_target2cam;
Mat actualRobotPos1 = (Mat_<double>(3, 1)),
actualRobotPos2 = (Mat_<double>(3, 1)),
actualRobotPos3 = (Mat_<double>(3, 1)),
actualRobotPos4 = (Mat_<double>(3, 1)),
actualRobotPos5 = (Mat_<double>(3, 1)),
actualRobotPos6 = (Mat_<double>(3, 1)),
actualRobotPos7 = (Mat_<double>(3, 1)),
actualRobotPos8 = (Mat_<double>(3, 1));
actualRobotPos1.at<double>(0,0) = -0.193139;
actualRobotPos1.at<double>(1,0) = 0.463823;
actualRobotPos1.at<double>(2,0) = -0.025;
t_gripper2base.push_back(actualRobotPos1);
actualRobotPos2.at<double>(0,0) = -0.193139;
actualRobotPos2.at<double>(1,0) = 0.463823;
actualRobotPos2.at<double>(2,0) = -0.025;
t_gripper2base.push_back(actualRobotPos2);
actualRobotPos3.at<double>(0,0) = -0.21273;
actualRobotPos3.at<double>(1,0) = 0.4426;
actualRobotPos3.at<double>(2,0) = -0.0288;
t_gripper2base.push_back(actualRobotPos3);
actualRobotPos4.at<double>(0,0) = -0.17213;
actualRobotPos4.at<double>(1,0) = 0.4103;
actualRobotPos4.at<double>(2,0) = 0.014;
t_gripper2base.push_back(actualRobotPos4);
actualRobotPos5.at<double>(0,0) = -0.13724;
actualRobotPos5.at<double>(1,0) = 0.45;
actualRobotPos5.at<double>(2,0) = 0.02978;
t_gripper2base.push_back(actualRobotPos5);
actualRobotPos6.at<double>(0,0) = -0.1655;
actualRobotPos6.at<double>(1,0) = 0.478;
actualRobotPos6.at<double>(2,0) = -0.0211;
t_gripper2base.push_back(actualRobotPos6);
actualRobotPos7.at<double>(0,0) = -0.17018;
actualRobotPos7.at<double>(1,0) = 0.46458;
actualRobotPos7.at<double>(2,0) = -0.03761;
t_gripper2base.push_back(actualRobotPos7);
actualRobotPos8.at<double>(0,0) = -0.193139;
actualRobotPos8.at<double>(1,0) = 0.463823;
actualRobotPos8.at<double>(2,0) = 0.025;
t_gripper2base.push_back(actualRobotPos8);
Mat actualRobotRotVec1 = (Mat_<double>(3, 1)),
actualRobotRotVec2 = (Mat_<double>(3, 1)),
actualRobotRotVec3 = (Mat_<double>(3, 1)),
actualRobotRotVec4 = (Mat_<double>(3, 1)),
actualRobotRotVec5 = (Mat_<double>(3, 1)),
actualRobotRotVec6 = (Mat_<double>(3, 1)),
actualRobotRotVec7 = (Mat_<double>(3, 1)),
actualRobotRotVec8 = (Mat_<double>(3, 1));
actualRobotRotVec1.at<double>(0,0) = -3.14159;
actualRobotRotVec1.at<double>(1,0) = 0.00;
actualRobotRotVec1.at<double>(2,0) = 0.00719124;
R_gripper2base.push_back(actualRobotRotVec1);
actualRobotRotVec2.at<double>(0,0) = -2.06;
actualRobotRotVec2.at<double>(1,0) = -2.36;
actualRobotRotVec2.at<double>(2,0) = 0.03;
R_gripper2base.push_back(actualRobotRotVec2);
actualRobotRotVec3.at<double>(0,0) = 2.39;
actualRobotRotVec3.at<double>(1,0) = 1.86;
actualRobotRotVec3.at<double>(2,0) = 0.49;
R_gripper2base.push_back(actualRobotRotVec3);
actualRobotRotVec4.at<double>(0,0) = -2.66;
actualRobotRotVec4.at<double>(1,0) = 0.08;
actualRobotRotVec4.at<double>(2,0) = 0.09;
R_gripper2base.push_back(actualRobotRotVec4);
actualRobotRotVec5.at<double>(0,0) = -2.84;
actualRobotRotVec5.at<double>(1,0) = 0.19;
actualRobotRotVec5.at<double>(2,0) = 0.69;
R_gripper2base.push_back(actualRobotRotVec5);
actualRobotRotVec6.at<double>(0,0) = 2.1;
actualRobotRotVec6.at<double>(1,0) = -2.34;
actualRobotRotVec6.at<double>(2,0) = -0.02;
R_gripper2base.push_back(actualRobotRotVec6);
actualRobotRotVec7.at<double>(0,0) = 1.66;
actualRobotRotVec7.at<double>(1,0) = -2.53;
actualRobotRotVec7.at<double>(2,0) = -0.23;
R_gripper2base.push_back(actualRobotRotVec7);
actualRobotRotVec8.at<double>(0,0) = -3.14159;
actualRobotRotVec8.at<double>(1,0) = 0.00;
actualRobotRotVec8.at<double>(2,0) = 0.00719124;
R_gripper2base.push_back(actualRobotRotVec8);
// for(int i = 0; i < t_gripper2base.size(); i++)
// {
// cout << t_gripper2base[i] << endl << endl;
// }
// for(int i = 0; i < R_gripper2base.size(); i++)
// {
// cout << R_gripper2base[i] << endl << endl;
// }
vector<String> fileNames;
glob("PATH*.png", fileNames, false); // directory of images
vector<vector<Point2f>> corners(fileNames.size());
Mat chessboardImg, chessboardImgGray;
vector<Point3f> objp;
vector<vector<Point3f>> worldCoordinates;
int checkerBoard[2] = {9,6};
double fieldSize = 0.008;
Mat cameraMatrixHandEye, distortionHandEye;
vector<Mat> rvecs, tvecs;
for(int i = 1; i < checkerBoard[1]; i++){
for(int j = 1; j < checkerBoard[0]; j++){
objp.push_back(Point3f(j*fieldSize, i*fieldSize, 0));
}
}
for(int i = 0; i < 8; i++)
{
chessboardImg = imread(fileNames[i]);
cvtColor(chessboardImg, chessboardImgGray, COLOR_BGR2GRAY);
bool patternFound = findChessboardCorners(chessboardImgGray, Size(8,5), corners[i], CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE);
if(patternFound)
{
cornerSubPix(chessboardImgGray, corners[i],Size(11,11), Size(-1,-1), TermCriteria(TermCriteria::EPS + TermCriteria::MAX_ITER, 30, 0.1));
drawChessboardCorners(chessboardImg, Size(8,5), corners[i], patternFound);
worldCoordinates.push_back(objp);
}
//***** Check loaded images and detected chessboard *****
//imshow("source", chessboardImgGray);
//imshow("chess", chessboardImg);
//waitKey(0);
//*******************************************************
}
float reprojectionError = calibrateCamera(worldCoordinates, corners, Size(1280,960), cameraMatrixHandEye, distortionHandEye, rvecs, tvecs, CALIB_FIX_ASPECT_RATIO + CALIB_FIX_K3 +
CALIB_ZERO_TANGENT_DIST + CALIB_FIX_PRINCIPAL_POINT);
//***** Check camera calibration results *****
//cout << "Reprojection Error CHE: " << endl << reprojectionError << endl << endl;
//cout << "Camera Matrix CHE: " << endl << cameraMatrixHandEye << endl << endl;
//cout << "Distortion CHE: " << endl << distortionHandEye << endl << endl;
//for(int i = 0; i < numberOfPoses; i++)
//{
// cout << "No. " << i+1 << " Target translation: " << endl << tvecs[i] << endl << endl;
// cout << "No. " << i+1 << " Target rotation: " << endl << rvecs[i] << endl << endl;
//}
//********************************************/
for(int i = 0; i < rvecs.size(); i++)
{
t_target2cam.emplace_back(tvecs[i]);
R_target2cam.emplace_back(rvecs[i]);
}
// for(int i = 0; i < t_target2cam.size(); i++)
// {
// cout << t_target2cam[i] << endl << endl;
// }
// for(int i = 0; i < R_target2cam.size(); i++)
// {
// cout << R_target2cam[i] << endl << endl;
// }
Mat R_cam2gripper;
Mat t_cam2gripper = (Mat_<double>(3, 1));
calibrateHandEye(R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, R_cam2gripper, t_cam2gripper);
cout << t_cam2gripper << endl << endl;
cout << R_cam2gripper << endl << endl;
Mat transformationMat4x4 = (Mat_<double>(4, 4));
Mat transformationMatInv4x4 = (Mat_<double>(4, 4));
Mat R_cam2gripperInv = (Mat_<double>(3, 3));
Mat t_cam2gripperInv = (Mat_<double>(3, 1));
transformationMat4x4.at<double>(0, 0) = R_cam2gripper.at<double>(0, 0);
transformationMat4x4.at<double>(0, 1) = R_cam2gripper.at<double>(0, 1);
transformationMat4x4.at<double>(0, 2) = R_cam2gripper.at<double>(0, 2);
transformationMat4x4.at<double>(0, 3) = t_cam2gripper.at<double>(0, 0);
transformationMat4x4.at<double>(1, 0) = R_cam2gripper.at<double>(1, 0);
transformationMat4x4.at<double>(1, 1) = R_cam2gripper.at<double>(1, 1);
transformationMat4x4.at<double>(1, 2) = R_cam2gripper.at<double>(1, 2);
transformationMat4x4.at<double>(1, 3) = t_cam2gripper.at<double>(1, 0);
transformationMat4x4.at<double>(2, 0) = R_cam2gripper.at<double>(2, 0);
transformationMat4x4.at<double>(2, 1) = R_cam2gripper.at<double>(2, 1);
transformationMat4x4.at<double>(2, 2) = R_cam2gripper.at<double>(2, 2);
transformationMat4x4.at<double>(2, 3) = t_cam2gripper.at<double>(2, 0);
transformationMat4x4.at<double>(3, 0) = 0;
transformationMat4x4.at<double>(3, 1) = 0;
transformationMat4x4.at<double>(3, 2) = 0;
transformationMat4x4.at<double>(3, 3) = 1;
transformationMatInv4x4 = transformationMat4x4.inv();
R_cam2gripperInv.at<double>(0,0) = transformationMatInv4x4.at<double>(0,0);
R_cam2gripperInv.at<double>(0,1) = transformationMatInv4x4.at<double>(0,1);
R_cam2gripperInv.at<double>(0,2) = transformationMatInv4x4.at<double>(0,2);
R_cam2gripperInv.at<double>(1,0) = transformationMatInv4x4.at<double>(1,0);
R_cam2gripperInv.at<double>(1,1) = transformationMatInv4x4.at<double>(1,1);
R_cam2gripperInv.at<double>(1,2) = transformationMatInv4x4.at<double>(1,2);
R_cam2gripperInv.at<double>(2,0) = transformationMatInv4x4.at<double>(2,0);
R_cam2gripperInv.at<double>(2,1) = transformationMatInv4x4.at<double>(2,1);
R_cam2gripperInv.at<double>(2,2) = transformationMatInv4x4.at<double>(2,2);
t_cam2gripperInv.at<double>(0,0) = transformationMatInv4x4.at<double>(0,3);
t_cam2gripperInv.at<double>(1,0) = transformationMatInv4x4.at<double>(1,3);
t_cam2gripperInv.at<double>(2,0) = transformationMatInv4x4.at<double>(2,3);
cout << transformationMatInv4x4 << endl << endl;
cout << t_cam2gripperInv << endl << endl;
Point3f objectPoints1, objectPoints2;
vector<Point3f> objectPoints;
objectPoints1.x = 0; //TCP in TCP-Coordinates
objectPoints1.y = 0;
objectPoints1.z = 0;
objectPoints.push_back(objectPoints1);
vector<Point2f> imagePoints;
projectPoints(objectPoints, R_cam2gripperInv, t_cam2gripperInv, defaultCameraMatrix, defaultDistortion, imagePoints);
cout << imagePoints[0] << endl << endl;
return a.exec();
}
`

Answer 1

you need to use solvepnp to get rvec and tvec for each image separately and then you will have a list of rvecs and tvecs. list length equals no of images. To get a similar list of rvec and tvec for Gripper_to_base transformation, you need to derive the R and T matrices based on robot dynamics which take rotation angle as input. Then for each pose you need to input the rotation angle data to R,T matrices to get rvec and tvec for each pose and make list of same length. Then you input them to calibrateHandeye function.

Answer 2

I could finally resolve the problem.
The calibration was correct, but there were two mistakes in my approach:
To find my TCP I used the TCP-Coordinates from the robot control. Instead I had to use (0,0,0), which is the TCP in the TCP-Coordinate-System.
The second mistake was to use the transformation matrix built from R and t out of calibrateHandEye. Instead I had to use the inverse transformation matrix.