精湛的企业网站建设,软文范文,郑州上街区网站建设公司,石家庄建设信息网官方网站三角测量
1. 核心公式推导
假设两个相机的投影矩阵为 P P P 和 P ′ P P′#xff0c;对应的匹配图像点(同名点)为 ( u , v ) (u, v) (u,v) 和 ( u ′ , v ′ ) (u, v) (u′,v′)#xff0c;目标是求解三维点 X [ X x , X y , X z , 1 ] T X [X_x, X_y, X_z, 1]^T X…三角测量
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} ⎩ ⎨ ⎧uP20XxP21XyP22XzP23P00XxP01XyP02XzP03vP20XxP21XyP22XzP23P10XxP11XyP12XzP13u′P20′XxP21′XyP22′XzP23′P00′XxP01′XyP02′XzP03′v′P20′XxP21′XyP22′XzP23′P10′XxP11′XyP12′XzP13′
消去分母
将分母移到等式左边得到四个线性方程 { 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(P20XxP21XyP22XzP23)P00XxP01XyP02XzP03v(P20XxP21XyP22XzP23)P10XxP11XyP12XzP13u′(P20′XxP21′XyP22′XzP23′)P00′XxP01′XyP02′XzP03′v′(P20′XxP21′XyP22′XzP23′)P10′XxP11′XyP12′XzP13′
矩阵形式
将上述方程整理为齐次方程组 A ⋅ X 0 A \cdot X 0 A⋅X0其中系数矩阵 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步骤2SVD分解求解最小二乘解
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)
