四元数正在翻转符号以进行非常相似的旋转？

Question

考虑以下最小的工作示例：

#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 代表相同的旋转 - 但是，它在视觉上并不吸引人，而且坦率地说很烦人，四元数会在其每个值中翻转符号。对于一般情况，如何纠正这一点（即四元数始终保持其“手性”，而不是在某些旋转时翻转符号）？

Answer 1

当单位四元数用于表示 3d 旋转时，有两种方法可以表示每个实际旋转 - 如果不在空间中创建人为的不连续性，就无法避免“负”的发生。

与单位圆上使用复数的 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

如果你只想检查它们是否在某个角度'th'内，你只需要结果的实部。这相当于找到 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 成为输入的 sum 的向量长度，而不是差）；然后你会得到 m

对于小 m，反正弦公式有泰勒级数

th ~=~ 2*m + (m^3)/12 + ...

因此，对于较小的增量，相对旋转角度大约是矢量差异幅度的两倍（这在数值上比当 w 接近 1 时使用 w 的反余弦更可靠）。

Answer 2

矩阵 1 的偏航角大于 90 度，矩阵 2 小于 90 度。这将导致偏航角的余弦对两者具有不同的符号，这会翻转您的四元数。

一个可能的解决方案是检查四元数的w 值。如果这是负数，你可以翻转它。

Answer 3

如果您可以访问前一个和当前的四元数读数，如果它使 4D 向量空间中的四元数之间的距离更小，您可以翻转当前四元数的符号。

翻转符号不会影响旋转，但会保证在旋转空间（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;
}