坐标系变换的坑

坐标系变换本来是很简单的事情，公式也很简单。但是卡了我很多天，原因是：两个坐标系的位姿，虽然都是右手系，但我的在顺时针旋转是yaw角是递增的，同事发给我的却是逆时针递减的。
理论上很简单的事情，搞了我那么久。看来工程是工程，理论是理论。

但这件事也教会了我一件事，不要把理论上的事情潜意识里当作大家都遵守。例如这次，我潜意识里默认大家都应该遵守右手坐标系，逆时针转动yaw角递增。

后面问了copilot，最后一句提醒了我要检查一下yaw角。

GitHub Copilot: 在右手坐标系中，逆时针旋转时，yaw角是递增的。这是因为在右手坐标系中，x轴指向右侧，y轴指向前方，z轴指向上方。当逆时针旋转时，相对于初始状态，车头朝向会逐渐向左侧旋转，即yaw角会逐渐增加。

需要注意的是，不同的坐标系约定可能不同，因此在使用yaw角时需要根据具体的坐标系约定来确定yaw角的递增方向。

将yaw角统一起来后，坐标系变换就正常了。下面是统一前后的对比：