对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())。

你可能感兴趣的:(#,点云重建,#,SLAM,python,T265,坐标变换,四元数)