多视图几何-恢复相机位姿内参的几种方法
多视图几何–恢复相机位姿/内参的几种方法
恢复相机位姿的几种方法
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
f
x
,
f
y
和主点坐标
( c x , c y ) (c_x, c_y)
(
c
x
,
c
y
) ，形式为：
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
=
f
x
0
0
0
f
y
0
c
x
c
y
1
[ 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
R
T
R
=
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
=
f
x
0
0
0
f
y
0
c
x
c
y
1
，则
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/
f
x
0
0
0
1/
f
y
0
−
c
x
/
f
x
−
c
y
/
f
y
1
。
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
′
T
F
x
=
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
=
K
T
F
K
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
Σ
V
T
其中
Σ
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
=
U
W
V
T
或
R
=
U
W
T
V
T
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
=
0
1
0
−
1
0
0
0
0
1
这会产生 四组候选解
( 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)
(
R
1
,
t
1
)
,
(
R
1
,
−
t
1
)
,
(
R
2
,
t
1
)
,
(
R
2
,
−
t
1
) 。
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
slam14讲