文章目录
- 三角测量（Triangulation）原理解析
-
- 一、基本定义
- 二、几何直观
- 三、基本数学模型
-
- [1. 齐次坐标投影方程](#1. 齐次坐标投影方程)
- [四、线性三角化（Linear Triangulation）](#四、线性三角化（Linear Triangulation）)
- 五、几何解释：射线交点
- [六、改进方法：最优三角化（Optimal Triangulation）](#六、改进方法：最优三角化（Optimal Triangulation）)
- 七、数值稳定性与注意事项
- [八、在 SfM / MVS 中的作用](#八、在 SfM / MVS 中的作用)
- 九、核心公式回顾
- 十、总结一句话
三角测量（Triangulation）原理解析
一、基本定义
三角测量（Triangulation） 是通过两台（或多台）相机的投影信息，计算空间点三维坐标的一种方法。
已知：
- 两个相机的投影矩阵 P 1 , P 2 P_1, P_2 P1,P2
- 同一空间点在两幅图像中的对应点 p 1 , p 2 p_1, p_2 p1,p2
目标是：
求出三维点 X = [ X , Y , Z , 1 ] T X = [X, Y, Z, 1]^{\mathsf{T}} X=[X,Y,Z,1]T 的坐标。
二、几何直观
从几何上看：
- 空间点 X X X 经过两个相机投影到图像上，形成 p 1 p_1 p1 和 p 2 p_2 p2
- 由相机中心 O 1 O_1 O1、 O 2 O_2 O2 及点 X X X 构成一个三角形
- 因此称为 "三角测量（Triangulation）"
三角测量的本质：
通过两条视线（射线）的交点来恢复空间点的三维位置。
三、基本数学模型
1. 齐次坐标投影方程
空间点 X X X 与图像点 p p p 的关系为：
p ∼ P X p \sim P X p∼PX
其中：
- P = K [ R ∣ t ] P = K [R | t] P=K[R∣t] 为相机投影矩阵
- p = [ x , y , 1 ] T p = [x, y, 1]^{\mathsf{T}} p=[x,y,1]T 为图像点的齐次坐标
- " ∼ \sim ∼" 表示相差一个比例因子 s s s（同线性）
s p = P X s p = P X sp=PX
比例因子 s s s 实际上就是该三维点在相机坐标系下的深度 Z c Z_c Zc。
对于两台相机：
{ p 1 ∼ P 1 X p 2 ∼ P 2 X \begin{cases} p_1 \sim P_1 X \\ p_2 \sim P_2 X \end{cases} {p1∼P1Xp2∼P2X
四、线性三角化（Linear Triangulation）
对于每个相机的齐次形式投影方程：
s i [ u i v i 1 ] = P i [ X Y Z 1 ] s_i \begin{bmatrix} u_i \\ v_i \\ 1 \end{bmatrix} = P_i \begin{bmatrix} X \\ Y \\ Z \\ 1 \end{bmatrix} si uivi1 =Pi XYZ1
其中， s i s_i si 是尺度因子（深度）。
则对于每一个相机 i i i 的投影方程：
每个方程有 3 个分量：
s i u i = p i 1 T X , s i v i = p i 2 T X , s i = p i 3 T X s_i u_i = p_{i1}^{\mathsf{T}} \mathbf{X}, \quad s_i v_i = p_{i2}^{\mathsf{T}} \mathbf{X}, \quad s_i = p_{i3}^{\mathsf{T}} \mathbf{X} siui=pi1TX,sivi=pi2TX,si=pi3TX
其中， p i 1 , , p i 2 , , p i 3 p_{i1},, p_{i2},, p_{i3} pi1,,pi2,,pi3 分别是 P i P_i Pi 的第 1、2、3 行向量。
通过消去 s i s_i si：
u i , ( p i 3 T X ) = p i 1 T X u_i ,(p_{i3}^{\mathsf{T}} \mathbf{X}) = p_{i1}^{\mathsf{T}} \mathbf{X} ui,(pi3TX)=pi1TX
v i , ( p i 3 T X ) = p i 2 T X v_i ,(p_{i3}^{\mathsf{T}} \mathbf{X}) = p_{i2}^{\mathsf{T}} \mathbf{X} vi,(pi3TX)=pi2TX
将两式改写为线性方程形式：
( u i , p i 3 T − p i 1 T ) X = 0 (u_i, p_{i3}^{\mathsf{T}} - p_{i1}^{\mathsf{T}}) \mathbf{X} = 0 (ui,pi3T−pi1T)X=0
( v i , p i 3 T − p i 2 T ) X = 0 (v_i, p_{i3}^{\mathsf{T}} - p_{i2}^{\mathsf{T}}) \mathbf{X} = 0 (vi,pi3T−pi2T)X=0
组合两个相机的两视图的方程，可得总共 4 个线性方程：
A X = 0 A \mathbf{X} = 0 AX=0
其中：
A = [ u 1 p 13 T − p 11 T v 1 p 13 T − p 12 T u 2 p 23 T − p 21 T v 2 p 23 T − p 22 T ] (矩阵尺寸 4 × 4 ) A = \begin{bmatrix} u_1 p_{13}^{\mathsf{T}} - p_{11}^{\mathsf{T}} \\ v_1 p_{13}^{\mathsf{T}} - p_{12}^{\mathsf{T}} \\ u_2 p_{23}^{\mathsf{T}} - p_{21}^{\mathsf{T}} \\ v_2 p_{23}^{\mathsf{T}} - p_{22}^{\mathsf{T}} \end{bmatrix} \quad \text{(矩阵尺寸 } 4 \times 4) A= u1p13T−p11Tv1p13T−p12Tu2p23T−p21Tv2p23T−p22T (矩阵尺寸 4×4)
解法：通过最小二乘求解 A X = 0 A X = 0 AX=0，即：
X = arg min ∣ X ∣ = 1 ∣ A X ∣ X = \arg\min_{|X|=1} |A X| X=arg∣X∣=1min∣AX∣
该问题可通过 奇异值分解（SVD） 得到：
A = U Σ V T ⇒ X = V ( : , 4 ) A = U \Sigma V^{\mathsf{T}} \quad \Rightarrow \quad X = V_{(:,4)} A=UΣVT⇒X=V(:,4)
即 V V V 的最后一列，对应最小奇异值。
通过线性三角化（Linear Triangulation），我们最终求得三维空间点的齐次坐标 X = [ X 1 , X 2 , X 3 , X 4 ] T X = [X_1, X_2, X_3, X_4]^{\mathsf{T}} X=[X1,X2,X3,X4]T
最后将齐次坐标归一化成：
X cart = 1 X 4 [ X 1 , X 2 , X 3 ] T = [ X , Y , Z ] X_{\text{cart}} = \frac{1}{X_4} [X_1, X_2, X_3]^{\mathsf{T}} = [X,Y,Z] Xcart=X41[X1,X2,X3]T=[X,Y,Z]
非齐次坐标 [ X , Y , Z , 1 ] T [X,Y,Z,1]^{\mathsf{T}} [X,Y,Z,1]T
五、几何解释：射线交点
从物理几何意义上，三角测量等价于两条射线的交点：
{ r 1 = O 1 + λ 1 d 1 r 2 = O 2 + λ 2 d 2 \begin{cases} r_1 = O_1 + \lambda_1 \mathbf{d}_1 \\ r_2 = O_2 + \lambda_2 \mathbf{d}_2 \end{cases} {r1=O1+λ1d1r2=O2+λ2d2
其中：
- O i O_i Oi 为相机中心
- d i \mathbf{d}_i di 为方向向量（由像素反投影得到）
理论上，两射线应在空间交于一点。
实际由于噪声存在，它们一般不会精确相交，因此取 最近点中点 作为估计点：
X = r 1 ( λ 1 ∗ ) + r 2 ( λ 2 ∗ ) 2 X = \frac{r_1(\lambda_1^*) + r_2(\lambda_2^*)}{2} X=2r1(λ1∗)+r2(λ2∗)
- r₁(λ₁*)：在射线 r₁ 上的某个点。
- r₂(λ₂*)：在射线 r₂ 上的某个点。这两个点之间的欧氏距离 ||r₁(λ₁*) - r₂(λ₂*)|| 是所有可能点对中最小的。
- 参数 λ₁* 和 λ₂* 就是使这个距离最小化的最优参数。
六、改进方法：最优三角化（Optimal Triangulation）
线性方法简单但对噪声敏感。
可通过最小化 重投影误差（Reprojection Error）获得最优结果：
min X ∑ i = 1 2 ∣ p i − π ( P i X ) ∣ 2 \min_{X} \sum_{i=1}^2 |p_i - \pi(P_i X)|^2 Xmini=1∑2∣pi−π(PiX)∣2
其中 π ( ⋅ ) \pi(\cdot) π(⋅) 表示从齐次坐标到像素坐标的投影操作。
该问题为非线性优化 ，可通过 高斯--牛顿法（Gauss--Newton） 或 Levenberg--Marquardt 优化求解。
七、数值稳定性与注意事项
-
归一化（Normalization） ：
在进行线性三角化前应对图像点坐标进行归一化，以减少数值误差。
-
视差角（Baseline） ：
当两相机的基线很短或视差很小，三角测量精度会显著下降。
-
非线性优化后精化 ：
通常先用线性三角化求初值，再以最小重投影误差方式进行迭代精化。
八、在 SfM / MVS 中的作用
| 应用场景 | 作用 |
|---|---|
| SfM（Structure from Motion） | 根据估计的相机姿态恢复三维点云 |
| MVS（Multi-View Stereo） | 利用多视图一致性进行密集三维重建 |
| 双目匹配（Stereo Matching） | 根据左右相机匹配点计算深度（ Z Z Z） |
九、核心公式回顾
| 名称 | 公式 | 含义 |
|---|---|---|
| 投影方程 | p ∼ P X p \sim P X p∼PX | 空间点到图像点的投影关系 |
| 线性方程 | A X = 0 A X = 0 AX=0 | 由两幅图像观测建立的约束 |
| SVD 解法 | X = V ( : , 4 ) X = V_{(:,4)} X=V(:,4) | 最小二乘意义下的三维点解 |
| 最优估计 | $\min_X \sum_i | p_i - \pi(P_i X) |
十、总结一句话
三角测量 = 由多视图投影恢复三维点坐标的过程。
它是从二维观测到三维几何的关键步骤，
与极线约束互为几何对应：
极线约束描述"两个点应在同一平面上"，
三角测量计算"该平面与两条视线的交点"。