Consider the following minimal working example:
#include <iostream>
#include <math.h>
#include <eigen3/Eigen/Dense>
int main() {
// Set the rotation matrices that give an example of the problem
Eigen::Matrix3d rotation_matrix_1, rotation_matrix_2;
rotation_matrix_1 << 0.15240781108708346, -0.98618841818279246, -0.064840288106743013,
-0.98826031445019891, -0.1527775600229907, 0.00075368177315370682,
-0.0106494132438156, 0.063964216524108775, -0.99789536976680049;
rotation_matrix_2 << -0.12448670851248633, -0.98805453458380521, -0.090836645094957508,
-0.99167686914182451, 0.12086367053038971, 0.044372968742129482,
-0.03286406263376359, 0.095604444636749664, -0.99487674792051639;
// Convert to Euler angles
Eigen::Vector3d euler_angles_1 = rotation_matrix_1.eulerAngles(2, 1, 0)*180.0f/M_PI;
Eigen::Vector3d euler_angles_2 = rotation_matrix_2.eulerAngles(2, 1, 0)*180.0f/M_PI;
// Convert to quaternion
Eigen::Quaternion<double> quaternion_1(rotation_matrix_1);
Eigen::Quaternion<double> quaternion_2(rotation_matrix_2);
// Print out results
std::cout << "Euler angles 1:\nyaw = " << euler_angles_1[0] << "\npitch = " << euler_angles_1[1] << "\nroll = " << euler_angles_1[2] << std::endl;
std::cout << "Quaternion 1:\nw = " << quaternion_1.w() << "\nx = " << quaternion_1.x() << "\ny = " << quaternion_1.y() << "\nz = " << quaternion_1.z() << std::endl;
std::cout << std::endl;
std::cout << "Euler angles 2:\nyaw = " << euler_angles_2[0] << "\npitch = " << euler_angles_2[1] << "\nroll = " << euler_angles_2[2] << std::endl;
std::cout << "Quaternion 2:\nw = " << quaternion_2.w() << "\nx = " << quaternion_2.x() << "\ny = " << quaternion_2.y() << "\nz = " << quaternion_2.z() << std::endl;
}
Whose output is:
Euler angles 1:
yaw = 98.767
pitch = 179.39
roll = -3.66759
Quaternion 1:
w = 0.020826
x = 0.758795
y = -0.650521
z = -0.0248716
Euler angles 2:
yaw = 82.845
pitch = 178.117
roll = -5.48908
Quaternion 2:
w = -0.0193663
x = -0.661348
y = 0.748369
z = 0.0467608
Both rotations are nearly identical (as given by the Euler angles). The expected behavior is that quaternion_2 will have values with same sign as quaternion_1, i.e. for the output to be:
Quaternion 2:
w = 0.0193663
x = 0.661348
y = -0.748369
z = -0.0467608
However, Eigen appears to "flip" the quaternion. I am aware that q and -q represent the same rotation - however, it is visually not appealing, and frankly annoying, that the quaternion would flip sign in each of its values. How can this be rectified for the general case (i.e. that the quaternion always preserves its "handedness", rather than flipping sign for certain rotations)?
When unit quaternions are used to represent 3d rotations, there are two ways to represent each actual rotation - and you can't avoid the 'negative' ones occuring without creating an artificial discontinuity in the space.
Unlike 2d rotations using complex numbers on a unit circle, the farthest point on the unit hypersphere from '0 rotation' has to be '360 degree rotation', not '180 degree'; since there is a 2d-space of possible 180 rotations which needs to be represented, whereas all 360-degree rotations are equivalent regardless of axis.
You can always 'canonicize' by changing the sign of the whole thing when the w component is negative. There will still be cases where w = 0, these all represent rotations by 180 - e.g. (0,0,1,0) and (0,0,-1,0) represent the same rotation.
And, (0.01, 0.99995,0,0,0) and (-0.01, 0.99995,0,0) represent rotations very close together, but if you change the second one to the equivalent (0.01,-0.99995,0,0) then they are far apart in the 4d vector space.
So, practically speaking, you can still have a concern when you want to find the difference between two rotations to see how close they are. Canonicizing the two individually may not help; you would generally want to flip signs as needed to make them as close as possible.
Or, to compare rotations q1,q2 : find the quaternion product q1 * q2.conj(); this gives the difference as a rotation quaternion; if it has w < 0, change its signs. For q1 and q2 close together (regardless of initial sign diffs) the result will always be fairly close to (1,0,0,0).
If you only want to check if they are within a certain angle 'th' of each other, you only need the real part of the result. This is equivalent to finding the dot product of q1,q2 (treating them as unit vectors in 4-space), then you check if the abs. value of the result >= cos(th/2).
Another way to find the relative angle: find the vector difference of the two unit vectors, and find the magnitude 'm' of that difference vector, (square root of the sum of squares) which will be in range [0,2]. Then find
th = 4*arcsin(m/2)
... and this will be 0 ... 2*pi.
In cases where m > sqrt(2), th > pi and you are getting the 'wrong side' result (also, the computation will have terrible numeric accuracy as m gets close to 2.0). So, in such cases, change one of the signs (i.e. make m the vector length of the sum of the inputs, rather than the difference); you will then have m <= sqrt(2), th <= pi.
For small m, the arcsin formula has the taylor series
th ~=~ 2*m + (m^3)/12 + ...
So, for small deltas, the relative rotation angle is approximately twice the magnitude of the vector difference (and this is numerically much more reliable than using an inverse-cosine-of-w when w is nearly 1).
The yaw angle is greater than 90 degrees for matrix 1, and less than 90 degrees for matrix 2. This will cause the cosine of the yaw angle to have different signs for the two, which is flipping your Quaternion.
A possible solution would be to check the w value of the Quaternion. If this is negative, you can flip it.
If you have access to the previous and the current quaternion reading, you can flip the sign of the current quaternion if it makes the distance between the quaternions in the 4D vector space smaller.
Flipping the sign will not affect the rotation, but it will ensure that there are no large jumps in 4D vector space when the rotation difference in rotation space (SO(3)) is small.
Quaternion avoidJumps(Quaternion q_Current, Quaternion q_Prev)
{
if ((q_Prev - q_Current).squaredNorm() < (q_Prev + q_Current).squaredNorm())
return -q_Current;
else
return q_Current;
}
来源：https://stackoverflow.com/questions/42428136/quaternion-is-flipping-sign-for-very-similar-rotations