如何从相机透视矩阵H中提取精确旋转参数（偏航、俯仰、滚转）

核心原理 #

透视变换矩阵H（3x3）和相机外参（旋转矩阵R、平移向量t）、内参矩阵K的关系可简化为：
H = s * K * [R₁ R₂ t]
其中s是缩放因子，R₁、R₂是旋转矩阵R的前两列（因目标区域为平面，可将世界坐标系设为该平面的z=0平面，R的第三列被消去）。我们的目标是从H反向推导出完整的R，再转换为欧拉角（Yaw/Pitch/Roll）。

分步实现 #

1. 归一化透视矩阵

先用内参矩阵K的逆对H做归一化，消去内参的影响：

import numpy as np

# 已知相机内参矩阵K
K = np.array([[fx, 0, cx],
              [0, fy, cy],
              [0, 0, 1]], dtype=np.float32)
# 已得到的透视变换矩阵H
H = ...

K_inv = np.linalg.inv(K)
H_normalized = K_inv @ H

2. 提取旋转矩阵前两列并估算缩放因子

从归一化后的矩阵中取出前两列向量a、b，根据旋转矩阵的正交性，R₁、R₂是单位正交向量，因此a = s*R₁、b = s*R₂，缩放因子s可取两向量模长的平均值：

a = H_normalized[:, 0]
b = H_normalized[:, 1]
s = (np.linalg.norm(a) + np.linalg.norm(b)) / 2.0

# 归一化得到R₁、R₂
R1 = a / s
R2 = b / s

3. 补全旋转矩阵并修正正交性

旋转矩阵的第三列R₃是R₁和R₂的叉乘，但由于计算误差，直接得到的矩阵可能不是严格正交的，用SVD分解修正：

R3 = np.cross(R1, R2)
R = np.column_stack((R1, R2, R3))

# SVD修正，确保矩阵正交且行列式为1（旋转矩阵的必要条件）
U, _, Vt = np.linalg.svd(R)
R_corrected = U @ Vt

# 处理反射情况（行列式为-1时修正）
if np.linalg.det(R_corrected) < 0:
    R_corrected[:, 2] *= -1

4. 旋转矩阵转欧拉角（Yaw/Pitch/Roll）

以右手坐标系、相机初始朝向为世界Z轴正方向为例，采用XYZ旋转顺序（Yaw绕世界Y轴，Pitch绕相机X轴，Roll绕相机Z轴）转换：

sy = np.sqrt(R_corrected[0,0]**2 + R_corrected[1,0]**2)
singular = sy < 1e-6

if not singular:
    roll = np.arctan2(R_corrected[2,1], R_corrected[2,2])
    pitch = np.arctan2(-R_corrected[2,0], sy)
    yaw = np.arctan2(R_corrected[1,0], R_corrected[0,0])
else:
    roll = np.arctan2(-R_corrected[1,2], R_corrected[1,1])
    pitch = np.arctan2(-R_corrected[2,0], sy)
    yaw = 0

# 可选：转换为角度值
yaw_deg = np.rad2deg(yaw)
pitch_deg = np.rad2deg(pitch)
roll_deg = np.rad2deg(roll)

针对虚拟相机的优化 #

虚拟相机环境中，H的计算无真实图像噪声，因此缩放因子s可直接取np.linalg.norm(a)（或b），正交化修正步骤可简化甚至省略，结果精度更高。

为什么不用solvePnP？ #

solvePnP会同时迭代估计旋转和平移，而你已知相机真实位置，分解H的方法无需迭代，直接通过线性运算推导，更高效且避免了不必要的参数估计。

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