多视图几何--结构恢复--三角测量

三角测量

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\cdot 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 = pt1
    u2, 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)

---