I am running opencv solvePnP using 3d points that I am passing in manually, and 2d image points from the corner of an aruco marker.
my 4 x 2d, and 4 x 3d points are as follows:
2d: [631.811, 359.303]
2d: [750.397, 364.894]
2d: [761.65, 448.106]
2d: [621.837, 440.352]
3d: [2.73788, -2.19532, 119.784]
3d: [-18.8274, -3.25251, 119.167]
3d: [-17.7176, -15.5556, 101.448]
3d: [3.84767, -14.4984, 102.065]
my camera matrix is:
[657.0030186215286, 0, 646.6970647943629;
0, 657.0030186215286, 347.1175117491306;
0, 0, 1]
When I run this, I get rvec and tvec:
rot: [3.542907682527767e-08;
9.321253763995684e-10;
3.141592612624269]
tran: [-2.599387317758439e-06;
7.428777431217353e-07;
-1.306287668720693e-06]
Already, this looks strange. Please see the image attached for an example of where the marker is in relation to the camera. Then, i run the rvec and tvec through a function to get the camera pose in relation to the marker as an Eigen::quaterniond
void GetCameraPoseEigen(cv::Vec3d tvecV, cv::Vec3d rvecV, Eigen::Vector3d &Translate, Eigen::Quaterniond &quats)
{
Mat R;
Mat tvec, rvec;
tvec = DoubleMatFromVec3b(tvecV);
rvec = DoubleMatFromVec3b(rvecV);
cv::Rodrigues(rvec, R); // R is 3x3
R = R.t(); // rotation of inverse
tvec = -R*tvec; // translation of inverse
Eigen::Matrix3d mat;
cv2eigen(R, mat);
Eigen::Quaterniond EigenQuat(mat);
quats = EigenQuat;
double x_t = tvec.at<double>(0, 0);
double y_t = tvec.at<double>(1, 0);
double z_t = tvec.at<double>(2, 0);
Translate.x() = x_t * 10;
Translate.y() = y_t * 10;
Translate.z() = z_t * 10;
}
The resulting quaternion is:
1.12774e-08 2.96705e-10 1 -2.04828e-08
Which is obviously wrong. But where is my mistake? Do my 2d - 3d correspondences look correct? Or is it my Eigen conversion?
Thank you.
EDIT:
I have adjusted the getCorners function based on the helpful comments below.
std::vector<cv::Point3f> getCornersInWorld(double side, cv::Vec3d rvec, cv::Vec3d tvec) {
//marker half size
double half_side = side / 2;
// compute rot_mat
cv::Mat rot_mat;
cv::Rodrigues(rvec, rot_mat);
// transpose of rot_mat
cv::Mat rot_mat_t = rot_mat.t();
//points in marker space
cv::Point3d cnr1(0, -half_side, half_side);
cv::Point3d cnr2(0, half_side, half_side);
cv::Point3d cnr3(0, half_side, -half_side);
cv::Point3d cnr4(0, -half_side, -half_side);
//to mat
cv::Mat cnr1Mat(cnr1);
cv::Mat cnr2Mat(cnr2);
cv::Mat cnr3Mat(cnr3);
cv::Mat cnr4Mat(cnr4);
//rotate points
cv::Mat pt_newMat1 = (-rot_mat_t * cnr1Mat);
cv::Mat pt_newMat2 = (-rot_mat_t * cnr2Mat);
cv::Mat pt_newMat3 = (-rot_mat_t * cnr3Mat);
cv::Mat pt_newMat4 = (-rot_mat_t * cnr4Mat);
// convert tvec to point
cv::Point3d tvec_3d(tvec[0], tvec[1], tvec[2]);
cv::Mat tvecMat(tvec_3d);
//rotate tvec
cv::Mat tvec_new = (-rot_mat_t * tvecMat);
cv::Point3d p(tvec_new);
//subtract tvec from all
pt_newMat1 = pt_newMat1 - tvec_new;
pt_newMat2 = pt_newMat2 - tvec_new;
pt_newMat3 = pt_newMat3 - tvec_new;
pt_newMat4 = pt_newMat4 - tvec_new;
//back to pnt
cv::Point3d p1(pt_newMat1);
cv::Point3d p2(pt_newMat2);
cv::Point3d p3(pt_newMat3);
cv::Point3d p4(pt_newMat4);
std::vector<cv::Point3f> retPnts;
retPnts.push_back(p1);
retPnts.push_back(p2);
retPnts.push_back(p3);
retPnts.push_back(p4);
return retPnts;
}
This gives me:
2d: [640.669, 489.541]
2d: [746.65, 479.212]
2d: [787.032, 547.899]
2d: [662.316, 561.966]
3d: [26.7971, 83.641, -78.1216]
3d: [28.0609, 91.2866, -57.9595]
3d: [31.636, 111.129, -65.7082]
3d: [30.3722, 103.484, -85.8702]
rot: [-0.4156437942693543;
1.047535551778523;
-1.063860719199172]
tran: [20.6613956945096;
-2.802643913661057;
239.6085645311116]
quat:
-0.187639 0.472901 -0.480271 0.71449
Which looks more sensible, but is still wrong. the resulting pose seems to be around 45 degrees off what I would expect, and on the wrong axis.
