如何将世界坐标系下的物体位姿转换至相机坐标系？

核心数学原理 #

位姿转换分为位置转换和旋转转换两部分，核心是通过相机的逆变换将物体的世界位姿映射到相机坐标系：

旋转转换（四元数形式） #

1. 将物体的世界欧拉角 (rx1, ry1, rz1) 转换为四元数 Q_obj
2. 将相机的世界欧拉角 (rx2, ry2, rz2) 转换为四元数 Q_cam
3. 物体在相机坐标系下的旋转四元数为：
   $$Q_{obj_cam} = Q_{cam}^{-1} \times Q_{obj}$$

旋转转换（旋转矩阵形式） #

1. 将物体/相机的欧拉角转换为对应的正交旋转矩阵 M_obj、M_cam
2. 相机坐标系下的物体旋转矩阵为：
   $$M_{obj_cam} = M_{cam}^T \times M_{obj}$$
   （注：正交矩阵的逆等于其转置）

位置转换 #

1. 计算物体相对于相机的世界空间偏移向量：
   $$\Delta P = (px1 - px2,\ py1 - py2,\ pz1 - pz2)$$
2. 将偏移向量通过相机的逆旋转变换到相机坐标系：
   $$P_{obj_cam} = Q_{cam}^{-1} \times \Delta P$$
   或用旋转矩阵：
   $$P_{obj_cam} = M_{cam}^T \times \Delta P$$

Python 实现方案 #

使用 numpy 和 scipy.spatial.transform 处理欧拉角与四元数的转换，默认采用**Z-X-Y（Unity风格）**的欧拉角旋转顺序，可根据实际需求调整：

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

def world_to_camera_pose(obj_euler, obj_pos, cam_euler, cam_pos, euler_order='zyx'):
    # 转换欧拉角为四元数
    obj_rot = R.from_euler(euler_order, obj_euler, degrees=True)
    cam_rot = R.from_euler(euler_order, cam_euler, degrees=True)
    
    # 计算相机坐标系下的旋转
    cam_rot_inv = cam_rot.inv()
    obj_cam_rot = cam_rot_inv * obj_rot
    obj_cam_euler = obj_cam_rot.as_euler(euler_order, degrees=True)
    
    # 计算相机坐标系下的位置
    delta_pos = np.array(obj_pos) - np.array(cam_pos)
    obj_cam_pos = cam_rot_inv.apply(delta_pos)
    
    return obj_cam_euler, obj_cam_pos

# 示例调用
obj_euler = (rx1, ry1, rz1)  # 替换为实际物体欧拉角（度数）
obj_pos = (px1, py1, pz1)    # 替换为实际物体位置
cam_euler = (rx2, ry2, rz2)  # 替换为实际相机欧拉角（度数）
cam_pos = (px2, py2, pz2)    # 替换为实际相机位置

cam_space_euler, cam_space_pos = world_to_camera_pose(obj_euler, obj_pos, cam_euler, cam_pos)
print(f"相机坐标系下物体欧拉角：{cam_space_euler}")
print(f"相机坐标系下物体位置：{cam_space_pos}")

Unity C# 实现方案 #

直接利用Unity内置的Transform API简化计算，自动适配Unity的坐标系规则（Y轴向上，Z轴向前）：

using UnityEngine;

public class PoseConverter : MonoBehaviour
{
    /// <summary>
    /// 将物体的世界位姿转换到相机坐标系
    /// </summary>
    /// <param name="objWorldPose">物体的世界位姿（欧拉角+位置）</param>
    /// <param name="cameraTransform">相机的Transform组件</param>
    /// <returns>相机坐标系下的欧拉角和位置</returns>
    public static (Vector3 camSpaceEuler, Vector3 camSpacePos) WorldToCameraPose((Vector3 euler, Vector3 pos) objWorldPose, Transform cameraTransform)
    {
        // 转换位置
        Vector3 camSpacePos = cameraTransform.InverseTransformPoint(objWorldPose.pos);
        
        // 转换旋转
        Quaternion objWorldRot = Quaternion.Euler(objWorldPose.euler);
        Quaternion camWorldRot = cameraTransform.rotation;
        Quaternion camSpaceRot = Quaternion.Inverse(camWorldRot) * objWorldRot;
        Vector3 camSpaceEuler = camSpaceRot.eulerAngles;
        
        return (camSpaceEuler, camSpacePos);
    }

    // 示例调用
    void ExampleUsage()
    {
        Vector3 objEuler = new Vector3(rx1, ry1, rz1); // 物体世界欧拉角
        Vector3 objPos = new Vector3(px1, py1, pz1);   // 物体世界位置
        Transform camera = Camera.main.transform;      // 获取主相机

        var result = WorldToCameraPose((objEuler, objPos), camera);
        Debug.Log($"相机坐标系下欧拉角：{result.camSpaceEuler}");
        Debug.Log($"相机坐标系下位置：{result.camSpacePos}");
    }
}

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