三.描述状态不简单：三维空间刚体运动

1.在机器人领域中，通常使用世界坐标系（World Coordinate System）和里程计坐标系（Odometry Coordinate System）来描述刚体运动。
世界坐标系是一个固定的参考坐标系，通常用于描述环境中的物体位置和姿态。在世界坐标系中，起始点和坐标轴的方向是固定的，不随刚体运动而改变。世界坐标系提供了一个绝对参考，用于表示刚体在环境中的位置和方向。里程计坐标系是相对于刚体自身的坐标系，基于里程计测量数据进行计算得出。里程计通常使用传感器（如编码器）来测量刚体的轮子转动和行驶距离，然后通过积分计算出刚体的位移和姿态变化。里程计坐标系的原点通常设置为刚体的起始位置，在刚体运动过程中，根据里程计数据进行更新。
2.欧式变换包括平移（Translation）和旋转（Rotation）两个主要组成部分。常用的表示方法是使用4x4的变换矩阵，它可以表示平移和旋转的组合。
假设有n个欧式变换矩阵，表示为T1, T2, ..., Tn，它们按照顺序进行累加。则累加的结果矩阵可以表示为：
T = T1 * T2 * ... * Tn
3.旋转矩阵、四元数、欧拉角和旋转向量都是常用的表示方法，用于描述三维空间中的旋转和平移。
旋转矩阵：旋转矩阵是一个3x3的正交矩阵，用于表示三维空间中的旋转。矩阵的每一列（或每一行）代表了旋转后的坐标轴方向，通过矩阵乘法将旋转矩阵与向量相乘，可以实现对向量的旋转操作。
四元数：四元数是一种复数扩展，用于表示旋转。它由一个实部和三个虚部组成，可以表示为四维向量 (w, x, y, z)。通过四元数和向量的乘法运算，可以实现对向量的旋转操作。四元数相对于旋转矩阵具有紧凑性和计算效率的优势，并且不存在奇异性问题。
欧拉角：欧拉角是一种常见的旋转表示方法，它使用三个角度来描述旋转。常见的欧拉角顺序有三个：绕固定轴顺序的旋转顺序（例如XYZ顺序），Tait-Bryan角（例如Roll-Pitch-Yaw顺序）和航向-俯仰-滚动顺序（例如YPR顺序）。尽管欧拉角易于理解和直观，但在某些情况下会出现奇异性和歧义性问题。
旋转向量：旋转向量是一个三维向量，用于表示旋转轴和旋转角度的组合。旋转向量的长度表示旋转角度大小，而方向表示旋转轴的方向。通过旋转向量和向量的叉乘运算，可以实现对向量的旋转操作。

e.g使用python里的numpy和scipy库进行姿态变换。首先将四元数转换为旋转矩阵，之后定义一个新的旋转矩阵并进行矩阵点乘；平移向量相加即可实现变换。

import numpy as np
from scipy.spatial.transform import Rotation

# 定义姿态变换参数: 在这里定义姿态变换参数，例如旋转矩阵R或欧拉角yaw/pitch/roll等 
# 以下是一个简单的旋转矩阵示例
# 对每个人体姿势数据进行姿态变换，以单个为例
q0, qx, qy, qz = [0.5834008455276489, -0.7853162288665771, 0.14548823237419128, -0.14749594032764435]

R_input = Rotation.from_quat([qx, qy, qz, q0]).as_matrix()
R_new = np.array([[0, -1, 0],
                  [1, 0, 0],
                  [0, 0, 1]])
R_output = np.dot(R_new, R_input)
q = Rotation.from_matrix(R_output).as_quat()

# 平移向量的修改
translation = [2044.45849609375, 4935.1171875, 1481.2275390625]
translation_new = np.array([0, 0, 1])
translation += translation_new

#输出修改后的数据：
print(q)
print(translation)

运行结果：