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?
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.
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.
You don't negate just w, you negate the whole quaternion.
So any time w is less than zero, I negate x, y, z and w?
.. if(quat.w() < 0) {quat = quat.Inverse();} <---- I have tried this, but see the same result.
Inverse != negating x,y,z, and w.
@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.501638Where 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.0198193RVEC 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.