# SolvePnp results to Quaternion, euler flipping.

I am using aruco markers and solvePnp to return A camera pose. I run PnP, then I use the following function to get the camera pose as a quaternion rotation from the rvec and tvec:

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;

}


This works, but i am still seeing huge flipping in the rotation values at some angles. My pose flips 180 degrees, and then back.

I am running the DrawAxis to check the pose in my camera frame, and this looks solid, yet my converted values do not. Where am i going wrong? Is there a mistake in the above function?

edit retag close merge delete

Remember that OpenCV is default Row Major, while Eigen is default Column Major. That means you may be transposing your rotation matrix. You should double check that. Or better, use the built in functions HERE.

Secondly, quaternions suffer from sign flipping. See HERE. So if that's what you're seeing, it's fine.

( 2018-03-17 12:36:50 -0500 )edit

Thank you. I have adjusted my function to: ... Eigen::Matrix3d mat; cv2eigen(R, mat);

Eigen::Quaterniond EigenQuat(mat);

quats = EigenQuat; ...

And I see the same thing. So I believe it is the flipping of positive and negative, as in the link you posted. I am assigning the rotation to a 3d model, and it twitches like crazy, so i need to smooth out the flips. I have tried comparing the previous and current quat, and negating the w component if it is negative when the previous was positive, but I still get flips. I know this is veering a little off openCv, but if you could point me at a way to get smooth quat data, it would be very much appreciated. Thanks again.

( 2018-03-17 12:43:22 -0500 )edit

You don't negate just w, you negate the whole quaternion.

( 2018-03-17 12:46:17 -0500 )edit

So any time w is less than zero, I negate x, y, z and w?

( 2018-03-17 12:48:51 -0500 )edit

.. if(quat.w() < 0) {quat = quat.Inverse();} <---- I have tried this, but see the same result.

( 2018-03-17 13:01:55 -0500 )edit
1

Inverse != negating x,y,z, and w.

( 2018-03-18 18:29:00 -0500 )edit

@Tetragramm, thank you. I have logged the data as the flip occurs, and see some strange behavior. With the code as in the question above, the flip looks like this: rvec: [-1.9554, 0.0461893, 0.762008] quat: 0.807827 -0.019082 -0.314805 -0.497945 rvec: [-1.94081, 0.0626131, 0.774592] quat: -0.803091 0.0259088 0.32052 0.501638 Where you can clearly see the quat flip, while the rvec values stay the same. When I (negate xyz if w <0), I get this: rvec: [2.87162, 0.466238, -1.13857] quat: 0.919152 0.149234 -0.364435 0.00875124 rvec: [-2.84968, -0.447527, 1.14067] quat: -0.918494 -0.144244 0.367654 -0.0198193 RVEC now flips. I have run this multiple times and see the same thing. Weird. (This is with an Aruco marker map, a complex object, on multiple planes.

( 2018-03-21 04:49:41 -0500 )edit

Sort by ยป oldest newest most voted

So you aren't having a problem. It's just different rotational representations. Both of your examples have essentially identical quaternions, if you negate one of the two.

It's very difficult to comprehend the contents of rvec. MRPs just don't make intuitive sense, in any particular form. But what I see from the quaternions is that there's no big changes from one to the next, just a negation.

more

The issue is, that the 'flip' from positive to negative is causing pops and flips in my animation. (i am streaming the data into Helix toolkit and Unreal engine, and in both, when the values flip, my animated object also flips). Can you think of a way to stop these values flipping like this? Would another solvePnP approach (EPnP, etc) perhaps help? Thanks again.

( 2018-03-22 04:40:35 -0500 )edit

I don't know what to say. Look at the rotation matrices. They're basically identical.

You can, if you're careful, average quaternions. So maybe do that in a running average to smooth things out?

( 2018-03-22 15:48:51 -0500 )edit

This issue was to do with quaternion unwinding, where, at the poles of each axis, the interpolation between positive and negative w values will take the longest path, instead of the shortest. more info here:

https://stackoverflow.com/questions/1...

( 2018-04-06 04:12:29 -0500 )edit

Official site

GitHub

Wiki

Documentation