基于OpenCV实现WHENet头部姿态欧拉角的相机-世界坐标系转换

核心问题分析 #

你遇到的问题本质是坐标系转换顺序错误和姿态投影逻辑混淆：

1. 相机外参T是世界→相机的变换矩阵，直接用R_camera_external @ R_object_camera会得到错误的旋转方向
2. draw_euler_axes中错误地用世界坐标系欧拉角旋转点，再通过相机外参投影，导致姿态显示混乱
3. 未明确WHENet、OpenCV相机坐标系、世界坐标系的轴定义匹配关系

关键修正点 #

1. 正确的旋转矩阵转换逻辑 #

WHENet输出的欧拉角是头部相对于相机坐标系的姿态，要转到世界坐标系，需遵循：
头部局部点 → 相机坐标系 → 世界坐标系
对应旋转矩阵的运算顺序为：

R_world_head = R_camera_world @ R_camera_head

其中：

• R_camera_world是相机外参旋转矩阵的转置（因为外参R_camera_external是世界→相机的旋转，正交矩阵的逆等于转置）
• R_camera_head是WHENet输出转换到相机坐标系的旋转矩阵（即你的initMat @ R_object_local）

2. 坐标轴投影的正确逻辑 #

不需要先旋转坐标轴点，直接在世界坐标系下生成头部的坐标轴，再通过相机外参投影到图像即可——cv2.projectPoints本身就是处理世界→图像的投影。

修正后的完整代码 #

import numpy as np
import cv2

def eulerAnglesToExtRef(eulerAngles, T, initMat=np.array([[1, 0, 0],[0, -1, 0],[0, 0, -1]])):
    # T是世界→相机的外参矩阵 (4x4)
    R_camera_external = T[:3, :3]
    # 相机→世界的旋转矩阵（外参旋转的逆变换）
    R_camera_world = R_camera_external.T
    
    # WHENet输出转相机坐标系下的头部旋转矩阵
    R_object_local = EpipolarUtils.euler_to_rotation_matrix(eulerAngles)
    R_object_camera = initMat @ R_object_local
    
    # 头部在世界坐标系下的旋转矩阵
    R_object_world = R_camera_world @ R_object_camera
    
    # 转欧拉角返回，同时返回旋转矩阵方便后续投影
    external_euler_angles = EpipolarUtils.rotation_matrix_to_euler(R_object_world)
    return external_euler_angles, R_object_world

def draw_euler_axes(img, world_origin, R_object_world, camera_matrix, distortion_coeffs, rvec, tvec, axis_length=0.2):
    # 在头部局部坐标系定义坐标轴点（x红, y绿, z蓝，对应前后方向）
    local_axes = np.array([
        [0, 0, 0],
        [axis_length, 0, 0],
        [0, axis_length, 0],
        [0, 0, axis_length]
    ], dtype=np.float32)
    
    # 转换为世界坐标系下的坐标轴点
    world_axes = (R_object_world @ local_axes.T).T + world_origin
    
    # 世界坐标系点投影到图像
    points_2D, _ = cv2.projectPoints(world_axes, rvec, tvec, camera_matrix, distortion_coeffs)
    points_2D = np.round(points_2D).astype(int).reshape(-1, 2)
    
    # 绘制坐标轴（x红, y绿, z蓝）
    axes_edges = [(0, 1), (0, 2), (0, 3)]
    axis_colors = [(0, 0, 255), (0, 255, 0), (255, 0, 0)]
    for i, edge in enumerate(axes_edges):
        pt1, pt2 = points_2D[edge[0]], points_2D[edge[1]]
        cv2.line(img, tuple(pt1), tuple(pt2), axis_colors[i], 2)

使用说明 #

1. 获取世界坐标系下的头部原点：先通过人脸关键点重建得到头部在相机坐标系下的3D位置camera_origin，再转换到世界坐标系：

   R_camera_world = camera_T[:3, :3].T
   world_origin = (R_camera_world @ camera_origin.T).T + camera_T[:3, 3].T
   

2. 调用转换函数：传入WHENet输出的欧拉角和相机外参，得到世界坐标系下的欧拉角和旋转矩阵：

   world_euler, R_world_head = eulerAnglesToExtRef(whenet_euler, camera_T)
   

3. 绘制坐标轴：传入世界原点、世界坐标系下的头部旋转矩阵、相机内参、外参的rvec/tvec即可。

验证方法 #

当人物正对相机时，世界坐标系下的头部z轴（蓝色）应与相机的视线方向（世界坐标系下的相机z轴）一致，此时图像中蓝色轴应指向镜头前方（你期望的前后方向）。

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