考虑以下最小的工作示例：
#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;
}
谁的输出是：
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
两个旋转几乎相同（由欧拉角给出）。预期的行为是quaternion_2将具有与quaternion_1相同的符号值，即输出为：
Quaternion 2:
w = 0.0193663
x = 0.661348
y = -0.748369
z = -0.0467608
然而，Eigen似乎“翻转”了四元数。我知道q和-q表示相同的旋转 - 但是，它在视觉上并不吸引人，并且坦率地说，四元数会翻转每个值的符号。如何纠正一般情况（即四元数总是保留其“手性”，而不是为某些旋转翻转符号）？
当单位四元数用于表示三维旋转时，有两种方法可以表示每个实际旋转 - 并且您无法避免在不创建空间中的人为不连续的情况下发生的“负”。
与在单位圆上使用复数的2d旋转不同，单位超球面上距离“0旋转”的最远点必须是“360度旋转”，而不是“180度”;因为存在需要表示的可能180次旋转的2d空间，而所有360度旋转都是等效的而不管轴。
当w分量为负时，你总是可以通过改变整个事物的符号来“规范化”。仍然存在w = 0的情况，这些都表示旋转180° - 例如（0,0,1,0）和（0,0，-1,0）表示相同的旋转。
并且，（0.01,0.99995,0,0,0）和（-0.01,0.99995,0,0）表示旋转非常接近，但如果将第二个变为等效（0.01，-0.99995,0,0）然后它们在4d向量空间中相距很远。
所以，实际上，当你想要找到两个旋转之间的差异时，你仍然可以有一个问题，看看它们有多接近。单独对两者进行封杀可能无济于事;你通常会想要根据需要翻转标志，使它们尽可能接近。
或者，比较旋转q1，q2：找到四元数乘积q1 * q2.conj（）;这给出了作为旋转四元数的差异;如果它<w，则改变其符号。对于q1和q2靠近在一起（无论初始符号差异），结果总是非常接近（1,0,0,0）。
如果您只想检查它们是否在彼此的某个角度内，您只需要结果的实际部分。这相当于找到q1，q2的点积（将它们视为4空间中的单位向量），然后检查是否为abs。结果的值> = cos（th / 2）。
找到相对角度的另一种方法是：找到两个单位矢量的矢量差，找到该差矢量的幅度'm'，（平方和的平方根）将在[0,2]范围内。然后找
th = 4*arcsin(m/2)
......这将是0 ... 2 * pi。
在m> sqrt（2），th> pi并且你得到'错误的一面'结果的情况下（同样，当m接近2.0时，计算将具有可怕的数字精度）。因此，在这种情况下，改变其中一个符号（即使得输入总和的矢量长度而不是差值）;然后你将m <= sqrt（2），th <= pi。
对于小m，arcsin公式具有泰勒系列
th ~=~ 2*m + (m^3)/12 + ...
因此，对于小的增量，相对旋转角度大约是矢量差值的两倍（当数值接近1时，这在数值上比使用反余弦的更可靠）。
对于矩阵1，偏航角大于90度，对于矩阵2，偏航角小于90度。这将导致偏航角的余弦具有不同的符号，这两个符号正在翻转四元数。
一种可能的解决方案是检查四元数的w值。如果这是否定的，你可以翻转它。
如果可以访问上一个四元数和当前四元数，则可以翻转当前四元数的符号（如果它使四维向量空间中四元数之间的距离更小）。
翻转符号不会影响旋转，但是当旋转空间（SO（3））的旋转差异很小时，它将确保4D向量空间中没有大的跳跃。
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;
}