对Intel T265输出的旋转四元数进行坐标变换

问题描述

• Intel T265会输出位置和角度(四元数)
• 在进行坐标变换的时候(例如，将T265的x-y-z变换为x-z-(-y)), 位置很容易进行变换，但是四元数角度的变换非常confusing

解决方案

上述旋转的代码如下

def convert_quaternion(q1):
    """
    Converts a quaternion representing a rotation in the x-y-z coordinate system to a quaternion
    representing the same rotation in the x-z-(-y) coordinate system.

    Args:
    q1: (x, y, z, w) quaternion representing a rotation in the x-y-z coordinate system.

    Returns:
    q2: (x, y, z, w) quaternion representing the same rotation in the x-z-(-y) coordinate system.
    """

    # Define a rotation that swaps the y and z axes and inverts the y axis
    swap_yz = R.from_euler('yxz', [0, 90, 0], degrees=True)

    # Convert q1 to a rotation object
    r1 = R.from_quat(q1)

    # Apply the coordinate system transformation and convert back to a quaternion
    r2 = swap_yz * r1 * swap_yz.inv()
    q2 = r2.as_quat()

    return q2

其中，比较难以理解的是r2 = swap_yz * r1 * swap_yz.inv()

• 这行代码的含义是：

  • 当要将一个旋转从一个坐标系转换到另一个坐标系时，需要按照以下步骤进行：
    • 将旋转应用于原始坐标系。
    • 将坐标系变换应用于结果。
    • 在这个例子中，已经有了 r1，它是在 x-y-z 坐标系中的一个旋转。现在想要在 x-z-(-y) 坐标系中表示相同的旋转。我们首先将坐标系变换（swap_yz）应用于 r1。这可以通过四元数乘法 swap_yz * r1 来完成。

• 然而，现在我们得到的旋转是在变换后的坐标系中的。我们需要将其转换回原始坐标系，以便我们可- 以比较两个旋转。这就是为什么我们需要乘以 swap_yz.inv() 的原因。swap_yz.inv() 是 swap_yz 的逆，它表示了从 x-z-(-y) 坐标系回到 x-y-z 坐标系的变换。

• 因此，r2 = swap_yz * r1 * swap_yz.inv() 这一行的意义是：

  • 将原始旋转 r1 变换到新的坐标系（swap_yz * r1）。
  • 将结果旋转从新的坐标系变换回原始坐标系（乘以 swap_yz.inv()）。