# OpenCv SolvePnp result, to Eigen Quaternion

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:

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 ...
edit retag close merge delete

How did you get those 3d points for the solvePnP? They aren't simple enough to draw conclusions from anything. Try setting them to z = 0, (x,y) whatever the offset is from the center of the marker to the corners.

( 2018-01-20 20:12:40 -0500 )edit

Hi, thanks for your reply. I am getting the 3d points by running solvePnP once (using the aruco module), then extrapolating the 3d points in world space from the marker pose. The reason for this is that I need to have multiple markers in frame, randomly placed, and run solvePnP on them.

When I add the points manually, for this one marker, like this:

cv::Point3d cnr1(0, -10, 10);
cv::Point3d cnr2(0, 10, 10);
cv::Point3d cnr3(0, 10, -10);
cv::Point3d cnr4(0, -10, -10);


I get:

rot: [0.4002261041215546;
0.4615760012941126;
-1.49600332980762]
tran: [6.236101764494571;
15.08829724022298;
101.9666936722527]

-0.17904 -0.206485 0.669233 0.690968


which looks correct. So I must have my 3d points wrong? Or have them in the wrong order?

( 2018-01-21 05:20:53 -0500 )edit

std::vector<cv::point3f> getCornersInCameraWorld(double side, cv::Vec3d rvec, cv::Vec3d tvec) {

        double half_side = markerLength / 2;
cv::Mat rot_mat;
cv::Rodrigues(rvec, rot_mat);

cv::Mat rot_mat_t = rot_mat.t();

double * tmp = rot_mat_t.ptr<double>(0);
cv::Point3f camWorldE(tmp[0] * half_side,
tmp[1] * half_side,
tmp[2] * half_side);

tmp = rot_mat_t.ptr<double>(1);
cv::Point3f camWorldF(tmp[0] * half_side,
tmp[1] * half_side,
tmp[2] * half_side);

// convert tvec to point
cv::Point3f tvec_3f(tvec[0], tvec[1], tvec[2]);

// return vector:
std::vector<cv::Point3f> ret(4, tvec_3f);
std::vector<cv::Point3f> ret2(4);

ret[0] += camWorldE + camWorldF;
ret[1] += -camWorldE + camWorldF;
ret[2] += -camWorldE - camWorldF;
ret[3] += camWorldE - camWorldF;
return ret;
}

( 2018-01-21 05:23:35 -0500 )edit

that is my function to get the world space corners from rvec and tvec. Thanks again!

( 2018-01-21 05:24:49 -0500 )edit
1

I can't quite follow what's going on, but it doesn't look right. You definitely need all the information in the rotation matrix.

First, invert the rotation matrix

Mat rot_mat_t = rot_mat.t();


Second, rotate all the points, including the tvec.

pt_new = (-rot_mat_t * pt_old)


Third, subtract the tvec from everything, which leaves the camera center at (0,0,0), and everything else positioned appropriately.

( 2018-01-21 16:47:55 -0500 )edit

Thank you! To clarify, pt_old, above, is the pt (0, - halfmarkersize, halfmarkersize) etc? So basically, set the points as if the middle of the marker is 0, rotate them all with the rotation matrix, and subtract tvec. Is that right?

( 2018-01-21 17:07:39 -0500 )edit

All the points you are interested in. You also need to do the same with tvec, and what you subtract is the tvec_new.

( 2018-01-21 17:15:58 -0500 )edit

I have updated my code, and added to the question above. It looks better, but still not correct. If you have a moment, could you take a look over the new function please? Thank you again for your time!

( 2018-01-22 13:45:04 -0500 )edit
1

Well, if the camera is (0,0,0), the marker points should all have a +z value. So if they're negative, something is wrong. I'll try to take a look later, depending on how much I get done.

( 2018-01-23 18:05:46 -0500 )edit

Thank you again, I am still stuck on this.

( 2018-01-24 09:31:24 -0500 )edit