C# System.Numerics实现ISO 8855-2011坐标系姿态补偿问题

问题背景 #

• 传感器遵循ISO 8855-2011坐标系，输出物体数据，原点为[0,0,0]但存在偏差，需补偿翻滚角（Roll）、俯仰角（Pitch）和偏航角（Yaw）。
• 使用System.Numerics库，以Vector3[x,y,z]存储数据点，尝试通过Matrix4x4或Quaternion实现旋转补偿。分步旋转（依次用Roll、Pitch、Yaw对应的四元数变换向量）的可视化效果符合预期，但代码冗余。
• 尝试用Quaternion.CreateFromYawPitchRoll单步实现时，测试了大量参数顺序和向量分量组合（约288种），均无法匹配分步旋转的结果；也未找到该方法的旋向性与旋转顺序说明。尝试四元数相乘（rotRoll * rotPitch * rotYaw）同样未得到预期效果。

解决方案 #

经过测试，以下代码可实现与分步旋转一致的效果：

// 创建RPY旋转四元数
Quaternion rot = Quaternion.CreateFromYawPitchRoll(yawAngle, pitchAngle, rollAngle);
// 转换向量分量后应用旋转
var vecRot = Vector3.Transform(new Vector3(Y, Z, X), rot);

再将旋转后的向量转换回目标坐标系的分量顺序：

var vecRes = new Vector3(vecRot.Z, vecRot.X, vecRot.Y);

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