从Projection Matrix求解相机位姿时的参数分解异常求助

你通过世界坐标-像素对求解的投影矩阵P投影像素误差小，但分解后内参K、外参R/T异常，逆推世界坐标失败，可从以下核心环节排查：

1. 投影矩阵P的尺度归一化 #

求解AP=0得到的P是尺度不确定的（P与kP等价，k为任意非零常数），直接分解会导致K、R、T的尺度完全错误。必须先对P做归一化处理：

import numpy as np
# 对P进行尺度归一化，确保前3×3子矩阵行列式为正（K对角线需为正）
det = np.linalg.det(P[:3, :3])
scale = np.sign(det) * np.abs(det)**(-1/3)
P = P * scale

2. 用RQ分解替代普通SVD分解 #

投影矩阵的标准形式是P = K[R | t]，其中K为上三角内参矩阵，R为正交旋转矩阵。直接用SVD分解无法保证K的上三角约束，必须用RQ分解：

import scipy.linalg as la
# 提取P的前3×3子矩阵M=KR
M = P[:, :3]
# 对M做RQ分解
K, R = la.rq(M)
# 修正K的对角线符号（确保焦距为正）
diag_sign = np.sign(np.diag(K))
K = K @ np.diag(diag_sign)
R = np.diag(diag_sign) @ R
# 计算平移向量t：t = K⁻¹ P[:,3]
t = np.linalg.inv(K) @ P[:, 3]

若分解后K的斜切项K[0,1]仍不为0，说明原始P存在噪声，可通过非线性优化强制约束K[0,1]=0、主点v0=540等先验条件，修正K、R、T。

3. 焦距的单位混淆问题 #

你提到的4mm是物理焦距，而分解得到的fx/fy是像素焦距，两者转换关系为：
fx = f_physical * (image_width_pixels / sensor_width_mm)
fy = f_physical * (image_height_pixels / sensor_height_mm)
比如1920×1080图像搭配6mm宽的传感器，4mm物理焦距对应的像素焦距约为4*(1920/6)=1280，而非4mm本身，别混淆两者单位。

4. 逆推世界坐标的步骤校验 #

两种逆推方法的正确步骤：

• 投影矩阵伪逆法：
  齐次像素坐标p=[u,v,1]^T，齐次世界坐标X=[X,Y,Z,1]^T满足p ~ PX，伪逆求解后需除以齐次分量得到3D坐标：

  X_homo = np.linalg.pinv(P) @ p
  X_world = X_homo[:3] / X_homo[3]
  

• K/R/T逆推法：
  1. 将像素坐标转换为归一化图像坐标：x=(u-K[0,2])/K[0,0]，y=(v-K[1,2])/K[1,1]
  2. 相机坐标系下的射线方向为[x,y,1]^T（需结合深度才能得到准确3D点，无深度仅能得到射线）
  3. 转换到世界坐标系：X_world = R.T @ (X_cam - t)（R是正交矩阵，R⁻¹=R.T）
     注意：若投影方程为p ~ K(RX + t)，则t是世界原点在相机坐标系下的向量，相机在世界坐标系中的位置是-R.T @ t，别搞反平移向量的定义。

5. 相机位姿可视化的坐标转换 #

光轴（+Z轴）的可视化需注意坐标转换逻辑：

• 相机坐标系的+Z轴在世界坐标系中的方向是R.T @ [0,0,1]^T
• 相机原点在世界坐标系中的位置是-R.T @ t
  如果光轴未指向世界原点，要么是R/T分解错误，要么是可视化时的坐标转换逻辑搞反了。

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