Point Cloud Library (PCL) for Python - pclpy 指南 (3) - SLAM 姿态数据格式 TUM 介绍

TUM定义

姿态文件的TUM格式来源于德国慕尼黑工业大学（Technische Universität München, TUM），该格式通常用于记录机器人或物体的轨迹信息。TUM格式的姿态数据文件包含时间戳、位置和四元数表示的姿态信息。每个字段的单词全称为：

1. timestamp: Time Stamp
2. x: Position X
3. y: Position Y
4. z: Position Z
5. qx: Quaternion X
6. qy: Quaternion Y
7. qz: Quaternion Z
8. qw: Quaternion W

 

数据格式

完整的一行数据表示为：

Time Stamp, Position X, Position Y, Position Z, Quaternion X, Quaternion Y, Quaternion Z, Quaternion W

例如：

1702910838.487092018 0.000366 0.005611 -0.000728 0.000105 -0.000093 -0.000478 0.999999876

表示的含义是：

1. 1702910838.487092018: 时间戳（Time Stamp），记录该位姿信息的时间。
2. 0.000366: X 方向上的位置（Position X）。
3. 0.005611: Y 方向上的位置（Position Y）。
4. -0.000728: Z 方向上的位置（Position Z）。
5. 0.000105: 四元数 X 分量（Quaternion X）。
6. -0.000093: 四元数 Y 分量（Quaternion Y）。
7. -0.000478: 四元数 Z 分量（Quaternion Z）。
8. 0.999999876: 四元数 W 分量（Quaternion W）。

这些字段共同描述了物体在某一时刻的三维位置和旋转姿态。

四元数说明

四元数用于表示三维空间中的旋转。相比欧拉角，四元数避免了万向节锁（Gimbal Lock）问题，并且在插值计算中表现更好。四元数由四个分量组成：qx, qy, qz, qw，它们满足以下关系：

$$q{x}^2+q{y}^2+q{z}^2+q{w}^2=1$$

使用四元数计算旋转

四元数可以通过以下公式转换为欧拉角（以获取旋转角度）：

$$\text{roll}=\text{atan2}(2(qwqx+qyqz),1-2(q{x}^2+q{y}^2))$$

$$\text{pitch}=\text{asin}(2(qwqy-qzqx))$$

$$\text{yaw}=\text{atan2}(2(qwqz+qxqy),1-2(q{y}^2+q{z}^2))$$

 

代码实现

在使用PCL（Point Cloud Library）的Python接口 pclpy 时，可以编写一个函数来将四元数转换为欧拉角。PCL本身并没有直接提供将四元数转换为欧拉角的功能，但可以使用 numpy 库来实现这一功能。

Point Cloud Library (PCL) for Python - pclpy 指南 (1) - 安装步骤

下面是使用 pclpy 和 numpy 实现的 quaternionToEuler 函数：

import numpy as np
import pclpy
from pclpy import pcl

def quaternionToEuler(qx, qy, qz, qw):
    """
    Converts quaternion (qx, qy, qz, qw) to Euler angles (roll, pitch, yaw).
    """
    # Calculate the Euler angles from the quaternion
    # roll (x-axis rotation)
    sinr_cosp = 2 * (qw * qx + qy * qz)
    cosr_cosp = 1 - 2 * (qx * qx + qy * qy)
    roll = np.arctan2(sinr_cosp, cosr_cosp)

    # pitch (y-axis rotation)
    sinp = 2 * (qw * qy - qz * qx)
    if abs(sinp) >= 1:
        pitch = np.sign(sinp) * np.pi / 2  # use 90 degrees if out of range
    else:
        pitch = np.arcsin(sinp)

    # yaw (z-axis rotation)
    siny_cosp = 2 * (qw * qz + qx * qy)
    cosy_cosp = 1 - 2 * (qy * qy + qz * qz)
    yaw = np.arctan2(siny_cosp, cosy_cosp)

    return roll, pitch, yaw

# Example usage:
qx, qy, qz, qw = 0.000105, -0.000093, -0.000478, 0.999999876
roll, pitch, yaw = quaternionToEuler(qx, qy, qz, qw)

print(f"Roll: {roll}, Pitch: {pitch}, Yaw: {yaw}")

代码解释

• 四元数: qx, qy, qz, qw 表示物体的旋转。
• 欧拉角: roll, pitch, yaw 分别表示绕 X、Y 和 Z 轴的旋转角度。绕X轴的滚转角(roll)，绕Y轴的俯仰角(pitch)，绕Z轴的偏航角(yaw)

详细说明

1. roll: 绕 X 轴的旋转，使用 atan2 计算。
2. pitch: 绕 Y 轴的旋转，使用 arcsin 计算，如果 sinp 超出范围，则返回 ±90 度。
3. yaw: 绕 Z 轴的旋转，使用 atan2 计算。

这个函数使用了 numpy 库的数学函数来计算欧拉角。

 

总结

TUM姿态数据格式提供了一种标准化的方式来记录机器人和物体在三维空间中的位姿信息。通过将四元数转换为欧拉角，可以更直观地理解这些姿态信息。在实际应用中，这些数据可以用于路径规划、运动控制和机器人导航等多个领域。

 

Point Cloud Library (PCL) for Python - pclpy 指南 (4) - ROS 输出 TUM 格式姿态数据