恢复相机位姿的几种方法
1分解投影矩阵
1.1投影矩阵分解为相机内外参矩阵的完整解析
投影矩阵（Projection Matrix）是计算机视觉中将三维世界点映射到二维像素坐标的核心工具，其本质是相机内参矩阵（Intrinsic Matrix）和外参矩阵（Extrinsic Matrix）的联合作用。
一、投影矩阵的数学构成
投影矩阵 P P P 是一个 3 × 4 3 \times 4 3×4 的矩阵，其表达式为：
P = K ⋅ R ∣ t P = K \cdot R \\ \| \\ t P=K⋅R ∣ t
其中：
- K K K 是内参矩阵（ 3 × 3 3 \times 3 3×3），包含焦距 f x , f y f_x, f_y fx,fy 和主点坐标 ( c x , c y ) (c_x, c_y) (cx,cy)，形式为：
K = f x 0 c x 0 f y c y 0 0 1 K = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} K= fx000fy0cxcy1 - R ∣ t R \\ \| \\ t R ∣ t 是外参矩阵（ 3 × 4 3 \times 4 3×4），由旋转矩阵 R R R（ 3 × 3 3 \times 3 3×3）和平移向量 t t t（ 3 × 1 3 \times 1 3×1）组成，表示世界坐标系到相机坐标系的变换。
关键性质：
- 投影矩阵前三列 P : , 1 : 3 P_{:,1:3} P:,1:3 对应 K ⋅ R K \cdot R K⋅R，第四列 P : , 4 P_{:,4} P:,4 对应 K ⋅ t K \cdot t K⋅t。
- 任何非奇异的前三列矩阵均可通过分解唯一确定 K K K 和 R R R。
二、分解步骤与数学方法
1. 分离内参矩阵 K K K 和外参旋转矩阵 R R R
对投影矩阵的前三列进行 RQ分解 （或等效的 QR分解 ）：
K ⋅ R = P : , 1 : 3 ⇒ RQ分解 ⇒ K , R K \cdot R = P_{:,1:3} \quad \Rightarrow \quad \text{RQ分解} \quad \Rightarrow \quad K, R K⋅R=P:,1:3⇒RQ分解⇒K,R
- RQ分解 ：将矩阵分解为一个上三角矩阵（对应内参 K K K）和一个正交矩阵（对应旋转 R R R）。由于 K K K 是上三角矩阵， R R R 是正交矩阵（满足 R T R = I R^T R = I RTR=I），此分解是唯一的。
- 验证分解结果 ：
- 检查 K K K 的最后一行为 0 , 0 , 1 0, 0, 1 0,0,1，否则需对矩阵进行归一化。
- 若分解后 R R R 的行列式不为 1 1 1（即非旋转矩阵），需调整符号以保证其为合法旋转矩阵。
2. 求解平移向量 t t t
从投影矩阵第四列提取 K ⋅ t K \cdot t K⋅t，并通过逆运算得到平移向量：
t = K − 1 ⋅ P : , 4 t = K^{-1} \cdot P_{:,4} t=K−1⋅P:,4
示例 ：若 K = f x 0 c x 0 f y c y 0 0 1 K = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} K= fx000fy0cxcy1 ，则 K − 1 = 1 / f x 0 − c x / f x 0 1 / f y − c y / f y 0 0 1 K^{-1} = \begin{bmatrix} 1/f_x & 0 & -c_x/f_x \\ 0 & 1/f_y & -c_y/f_y \\ 0 & 0 & 1 \end{bmatrix} K−1= 1/fx0001/fy0−cx/fx−cy/fy1 。
3. 分解的唯一性与约束条件
- 尺度等价性 ：投影矩阵 P P P 的尺度不确定性（即 P P P 和 λ P \lambda P λP 等价）需通过附加约束（如焦距的物理单位）确定。
- 非奇异条件 ：分解要求 P P P 的前三列矩阵非奇异（即 det ( P : , 1 : 3 ) ≠ 0 \det(P_{:,1:3}) \neq 0 det(P:,1:3)=0）。
2分解单应矩阵
2.1世界坐标到像素坐标
当世界坐标为平面时，投影矩阵此时为特殊的单应矩阵，利用张正友标定法原理可以求解相机位姿。
2.2像素坐标到像素坐标
当单应矩阵描述两张照片的射影关系时，有数值法和解析法：
《Motion and structure from motion in a piecewise planar environment》
《3d reconstruction based on homography mapping》
《Deeper understanding of the homography decomposition for vision based control》
3分解本质矩阵
3.1 基本矩阵分解得到相机位姿的完整解析
在双目视觉或多视图几何中，基本矩阵（Fundamental Matrix）描述了不同视角间图像点对应的对极几何约束关系。分解基本矩阵以恢复相机位姿（旋转矩阵 R R R和平移向量 t t t）是三维重建与SLAM系统的核心步骤。以下结合数学推导、分解方法及工程实践，详细阐述其实现过程。
一、基本矩阵与本质矩阵的关系
基本矩阵 F F F和本质矩阵 E E E是理解对极几何的关键：
-
基本矩阵 F F F ：
定义两视图间的对极约束关系，满足：
x ′ T F x = 0 \mathbf{x}'^T F \mathbf{x} = 0 x′TFx=0其中 x \mathbf{x} x和 x ′ \mathbf{x}' x′为归一化平面坐标点。
-
本质矩阵 E E E ：
当相机内参 K K K已知时， E E E与 F F F的关系为：
E = K T F K E = K^T F K E=KTFK
E E E可分解为旋转矩阵 R R R和平移向量 t t t的组合：
E = t × R E = t_\times R E=t×R其中 t × t_\times t×为平移向量 t t t的斜对称矩阵。
二、分解本质矩阵的数学步骤
1. 奇异值分解（SVD）
对本质矩阵 E E E进行SVD分解：
E = U Σ V T E = U \Sigma V^T E=UΣVT
其中 Σ = diag ( σ 1 , σ 2 , 0 ) \Sigma = \text{diag}(\sigma_1, \sigma_2, 0) Σ=diag(σ1,σ2,0)， σ 1 ≈ σ 2 \sigma_1 \approx \sigma_2 σ1≈σ2。根据Hartley的归一化方法， Σ \Sigma Σ可替换为 diag ( 1 , 1 , 0 ) \text{diag}(1,1,0) diag(1,1,0)以消除尺度影响。
2. 构造候选解
分解后，旋转矩阵 R R R和平移向量 t t t的可能组合为：
R = U W V T 或 R = U W T V T R = U W V^T \quad \text{或} \quad R = U W^T V^T R=UWVT或R=UWTVT
t = U ⋅ 3 或 t = − U ⋅ 3 t = U_{\cdot 3} \quad \text{或} \quad t = -U_{\cdot 3} t=U⋅3或t=−U⋅3
其中：
W = 0 − 1 0 1 0 0 0 0 1 W = \begin{bmatrix} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 1 \end{bmatrix} W= 010−100001
这会产生四组候选解 ( R 1 , t 1 ) , ( R 1 , − t 1 ) , ( R 2 , t 1 ) , ( R 2 , − t 1 ) (R_1, t_1), (R_1, -t_1), (R_2, t_1), (R_2, -t_1) (R1,t1),(R1,−t1),(R2,t1),(R2,−t1)。
3. 解的唯一性筛选
通过正深度约束筛选正确解：
- 对匹配点对 x ↔ x ′ \mathbf{x} \leftrightarrow \mathbf{x}' x↔x′，计算三角化后的3D点 P P P在相机坐标系下的深度 Z Z Z。
- 若所有点的深度 Z Z Z均为正，则该解为物理可行解。
- 若存在多组可行解，需结合多视图几何或先验信息进一步判断。
4PnP方法
通过已知的3d点和对应的2d点，直接求解相机位姿，主要有直接线性变换，p3p，EPnP，BA等。
参考：
MVG