四元数转Euler angles时batch_size与输出形状不匹配报错排查

为啥会出这些错？ #

• 第一个RuntimeError: shape '[4, 3]' is invalid for input of size 3：你输入的四元数根本不是批量格式，比如传了个单个四元数（形状(4,)），代码却以为是(4,3)的张量，元素总数对不上，自然报错。
• 第二个AssertionError: Mismatch in batch_size and euler_angles shape：你的转换函数没处理好批量维度，比如循环处理每个样本时，没把结果拼接成(batch_size,3)，最后只返回了单个样本的欧拉角（形状(3,)），和预期的batch_size不匹配。

直接上修复步骤 #

1. 先把输入形状搞对 #

不管用numpy还是PyTorch，批量四元数必须是(batch_size,4)的形状。如果是单个四元数，先扩展维度：

# numpy示例
import numpy as np
single_quat = np.array([1,0,0,0])
batch_quat = single_quat[np.newaxis, :]  # 变成(1,4)

# PyTorch示例
import torch
single_quat = torch.tensor([1,0,0,0])
batch_quat = single_quat.unsqueeze(0)  # 变成(1,4)

2. 重写支持批量的转换函数 #

别写只处理单个四元数的函数，用向量化操作直接整批处理，既快又不会丢维度。以下是两种框架的实现（默认ZYX旋转顺序，也就是偏航-俯仰-滚转）：

numpy批量转换函数

def quat_to_euler_batch(quats):
    # quats输入形状：(batch_size,4)，四元数格式[w,x,y,z]
    w, x, y, z = quats[:,0], quats[:,1], quats[:,2], quats[:,3]
    
    # 计算滚转角（roll）
    sinr_cosp = 2 * (w*x + y*z)
    cosr_cosp = 1 - 2*(x**2 + y**2)
    roll = np.arctan2(sinr_cosp, cosr_cosp)
    
    # 计算俯仰角（pitch）
    sinp = 2*(w*y - z*x)
    pitch = np.arcsin(np.clip(sinp, -1, 1))  # 防止数值溢出
    
    # 计算偏航角（yaw）
    siny_cosp = 2*(w*z + x*y)
    cosy_cosp = 1 - 2*(y**2 + z**2)
    yaw = np.arctan2(siny_cosp, cosy_cosp)
    
    # 拼接成(batch_size,3)的结果
    return np.stack([roll, pitch, yaw], axis=1)

PyTorch批量转换函数

def quat_to_euler_batch(quats):
    # quats输入形状：(batch_size,4)，四元数格式[w,x,y,z]
    w, x, y, z = quats[:,0], quats[:,1], quats[:,2], quats[:,3]
    
    sinr_cosp = 2 * (w*x + y*z)
    cosr_cosp = 1 - 2*(x**2 + y**2)
    roll = torch.atan2(sinr_cosp, cosr_cosp)
    
    sinp = 2*(w*y - z*x)
    pitch = torch.asin(torch.clamp(sinp, -1, 1))
    
    siny_cosp = 2*(w*z + x*y)
    cosy_cosp = 1 - 2*(y**2 + z**2)
    yaw = torch.atan2(siny_cosp, cosy_cosp)
    
    return torch.stack([roll, pitch, yaw], dim=1)

3. 加个断言验证形状 #

调用函数后，加个断言确保输出形状符合预期，避免踩坑：

batch_size = 4
# 生成随机四元数并归一化（必须是单位四元数！）
quats = np.random.rand(batch_size,4)
quats = quats / np.linalg.norm(quats, axis=1, keepdims=True)

eulers = quat_to_euler_batch(quats)
assert eulers.shape == (batch_size,3), f"形状不匹配：得到{eulers.shape}，预期{(batch_size,3)}"

必看注意事项 #

• 四元数必须是单位长度，转换前一定要归一化，不然结果全错。
• 欧拉角的旋转顺序要和你的业务需求一致，上面用的是ZYX，要是需要XYZ，得调整计算逻辑。
• 别用for循环遍历每个样本处理，向量化操作不仅效率高，还能避免手动拼接时丢失batch维度的问题。

内容的提问来源于stack exchange，提问作者Redhwan