Quaternion is flipping sign for very similar rotations?
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 , ie 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 (ie 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 - eg (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).
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.
上一篇: C＃