三角測量

1. 核心公式推導

假設兩個相機的投影矩陣為 $P$ 和 $P^{\prime }$，對應的匹配圖像點(同名點)為 $(u,v)$ 和 $(u^{\prime },v^{\prime })$，目標是求解三維點 $X=[X_x,X_y,X_z,1{]}^T$（齊次坐標）。

投影方程

每個相機的投影方程可以表示為：
$$?\begin{array}{l}u=\frac{P_{00}{X}_x+P_{01}{X}_y+P_{02}{X}_z+P_{03}}{P_{20}{X}_x+P_{21}{X}_y+P_{22}{X}_z+P_{23}}\\ v=\frac{P_{10}{X}_x+P_{11}{X}_y+P_{12}{X}_z+P_{13}}{P_{20}{X}_x+P_{21}{X}_y+P_{22}{X}_z+P_{23}}\\ u^{\prime }=\frac{P_{00}^{\prime }{X}_x+P_{01}^{\prime }{X}_y+P_{02}^{\prime }{X}_z+P_{03}^{\prime }}{P_{20}^{\prime }{X}_x+P_{21}^{\prime }{X}_y+P_{22}^{\prime }{X}_z+P_{23}^{\prime }}\\ v^{\prime }=\frac{P_{10}^{\prime }{X}_x+P_{11}^{\prime }{X}_y+P_{12}^{\prime }{X}_z+P_{13}^{\prime }}{P_{20}^{\prime }{X}_x+P_{21}^{\prime }{X}_y+P_{22}^{\prime }{X}_z+P_{23}^{\prime }}\end{array}$$

消去分母

將分母移到等式左邊，得到四個線性方程：
$$?\begin{array}{l}u(P_{20}{X}_x+P_{21}{X}_y+P_{22}{X}_z+P_{23})=P_{00}{X}_x+P_{01}{X}_y+P_{02}{X}_z+P_{03}\\ v(P_{20}{X}_x+P_{21}{X}_y+P_{22}{X}_z+P_{23})=P_{10}{X}_x+P_{11}{X}_y+P_{12}{X}_z+P_{13}\\ u^{\prime }(P_{20}^{\prime }{X}_x+P_{21}^{\prime }{X}_y+P_{22}^{\prime }{X}_z+P_{23}^{\prime })=P_{00}^{\prime }{X}_x+P_{01}^{\prime }{X}_y+P_{02}^{\prime }{X}_z+P_{03}^{\prime }\\ v^{\prime }(P_{20}^{\prime }{X}_x+P_{21}^{\prime }{X}_y+P_{22}^{\prime }{X}_z+P_{23}^{\prime })=P_{10}^{\prime }{X}_x+P_{11}^{\prime }{X}_y+P_{12}^{\prime }{X}_z+P_{13}^{\prime }\end{array}$$

矩陣形式

將上述方程整理為齊次方程組 $A?X=0$，其中系數矩陣 $A$ 的每一行對應一個方程：
$$A=\left[\begin{array}{c}u{P}_2?P_0\\ v{P}_2?P_1\\ u^{\prime }{P}_2^{\prime }?P_0^{\prime }\\ v^{\prime }{P}_2^{\prime }?P_1^{\prime }\end{array}\right]$$

這里 $P_i$ 表示投影矩陣的第 $i$ 行（如 $P_0=[P_{00},P_{01},P_{02},P_{03}]$），進而通過SVD求解$X$的齊次坐標。

---

2. 代碼實現步驟

以下是基于公式的手動實現（Python + NumPy）：

步驟1：構造系數矩陣 $ A $

def triangulate_point(P1, P2, pt1, pt2):"""P1, P2: 3x4 投影矩陣pt1, pt2: 匹配的二維點 (u, v)"""u1, v1 = pt1u2, v2 = pt2# 構造矩陣A的每一行row1 = u1 * P1[2, :] - P1[0, :]  # u * P1[2] - P1[0]row2 = v1 * P1[2, :] - P1[1, :]  # v * P1[2] - P1[1]row3 = u2 * P2[2, :] - P2[0, :]  # u' * P2[2] - P2[0]row4 = v2 * P2[2, :] - P2[1, :]  # v' * P2[2] - P2[1]A = np.vstack([row1, row2, row3, row4])return A

步驟2：SVD分解求解最小二乘解

def solve_svd(A):# 奇異值分解U, S, Vt = np.linalg.svd(A)# V的最后一列對應最小奇異值的解X = Vt[-1, :]# 歸一化齊次坐標X = X / X[3]return X[:3]  # 返回三維坐標 (X, Y, Z)

---