c++ - 四元数是非常相似旋转的翻转符号吗？

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 vector 空间中相距很远。

因此，实际上，当您想找到两个旋转之间的差异以查看它们有多接近时，您仍然会担心。将两者单独规范化可能无济于事； 您通常希望根据需要翻转标志，使它们尽可能靠近。

或者，比较旋转 q1,q2:找到四元数乘积 q1 * q2.conj();这给出了旋转四元数的差异；如果 w < 0，改变它的符号。对于靠在一起的 q1 和 q2(不管初始符号差异如何)，结果将始终非常接近 (1,0,0,0)。

如果您只想检查它们是否在彼此之间的特定角度“th”内，您只需要结果的实部。这相当于找到 q1,q2 的点积(将它们视为 4 空间中的单位 vector )，然后检查 abs.结果的值 >= cos(th/2)。

---

找到相对角度的另一种方法:找到两个单位 vector 的 vector 差，并找到该差 vector 的大小“m”(平方和的平方根)，其范围为 [0, 2].然后找到

th = 4*arcsin(m/2)

... 这将是 0 ... 2*pi。

在 m > sqrt(2)、th > pi 的情况下，您会得到“错误的一面”结果(此外，当 m 接近 2.0 时，计算的数值精度会很差)。因此，在这种情况下，更改其中一个符号(即使 m 成为输入的和 的 vector 长度，而不是差值)；然后你将有 m <= sqrt(2), th <= pi.

对于small m，arcsin公式有泰勒级数

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

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