三角测量
1. 核心公式推导
假设两个相机的投影矩阵为 P P P 和 P ′ P' P′，对应的匹配图像点(同名点)为 ( u , v ) (u, v) (u,v) 和 ( u ′ , v ′ ) (u', v') (u′,v′)，目标是求解三维点 X = [ X x , X y , X z , 1 ] T X = [X_x, X_y, X_z, 1]^T X=[Xx,Xy,Xz,1]T（齐次坐标）。
投影方程
每个相机的投影方程可以表示为：
{ u = 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 = 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 ′ = 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 ′ = 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 ′ \begin{cases} 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' = \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}} \\ \end{cases} ⎩ ⎨ ⎧u=P20Xx+P21Xy+P22Xz+P23P00Xx+P01Xy+P02Xz+P03v=P20Xx+P21Xy+P22Xz+P23P10Xx+P11Xy+P12Xz+P13u′=P20′Xx+P21′Xy+P22′Xz+P23′P00′Xx+P01′Xy+P02′Xz+P03′v′=P20′Xx+P21′Xy+P22′Xz+P23′P10′Xx+P11′Xy+P12′Xz+P13′
消去分母
将分母移到等式左边，得到四个线性方程：
{ 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 ′ ( 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 ′ \begin{cases} 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' (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} \\ \end{cases} ⎩ ⎨ ⎧u(P20Xx+P21Xy+P22Xz+P23)=P00Xx+P01Xy+P02Xz+P03v(P20Xx+P21Xy+P22Xz+P23)=P10Xx+P11Xy+P12Xz+P13u′(P20′Xx+P21′Xy+P22′Xz+P23′)=P00′Xx+P01′Xy+P02′Xz+P03′v′(P20′Xx+P21′Xy+P22′Xz+P23′)=P10′Xx+P11′Xy+P12′Xz+P13′
矩阵形式
将上述方程整理为齐次方程组 A ⋅ X = 0 A \cdot X = 0 A⋅X=0，其中系数矩阵 A A A 的每一行对应一个方程：
A = [ u P 2 − P 0 v P 2 − P 1 u ′ P 2 ′ − P 0 ′ v ′ P 2 ′ − P 1 ′ ] A = \begin{bmatrix} u P_{2} - P_{0} \\ v P_{2} - P_{1} \\ u' P'_{2} - P'_{0} \\ v' P'_{2} - P'_{1} \\ \end{bmatrix} A= uP2−P0vP2−P1u′P2′−P0′v′P2′−P1′
这里 P i P_{i} Pi 表示投影矩阵的第 i i i 行（如 P 0 = [ P 00 , P 01 , P 02 , P 03 ] P_{0} = [P_{00}, P_{01}, P_{02}, P_{03}] P0=[P00,P01,P02,P03]），进而通过SVD求解 X X 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)
![3.10[A]cv](http://pic.xiahunao.cn/3.10[A]cv)
 创建并集成本地库)
（暴力破解；栈（从左到右）；栈（从右到左））)
)
