一、原理
二、步骤
a.用各自像片的角元素计算出左右像片的旋转矩阵R1和R2。
b.有同名像点列出共线方程。
c.将方程写为未知数的线性方程形式,计算线性系数。
d.写出误差方程,系数矩阵与常数项。
e.计算未知点的最小二乘解。
f.重复以上步骤完成所有点的地面坐标的计算。
三、示例代码
# -*- coding: utf-8 -*- """ Created on Mon Nov 25 09:38:08 2019 @author: L JL """ import numpy as np import math as m def r_mat(f,w,k): Rf = np.mat([[m.cos(f), 0, -m.sin(f)], [0, 1, 0], [m.sin(f), 0, m.cos(f)]]) Rw = np.mat([[1, 0, 0], [0, m.cos(w), -m.sin(w)], [0, m.sin(w), m.cos(w)]]) Rk = np.mat([[m.cos(k), -m.sin(k), 0], [m.sin(k), m.cos(k), 0], [0, 0, 1]]) R = Rf*Rw*Rk return R def l_mat(In,R,coor): l = np.mat(np.zeros((2,3))) f = In[0,2] xo = In[0,0] yo = In[0,1] x = coor[0] y = coor[1] l[0,0] = f*R[0,0] + (x-xo)*R[0,2] l[0,1] = f*R[1,0] + (x-xo)*R[1,2] l[0,2] = f*R[2,0] + (x-xo)*R[2,2] l[1,0] = f*R[0,1] + (y-yo)*R[0,2] l[1,1] = f*R[1,1] + (y-yo)*R[1,2] l[1,2] = f*R[2,1] + (y-yo)*R[2,2] return l def l_approximate(In,R,coor,Ex): l_app = np.mat(np.zeros((2,1))) f = In[0,2] xo = In[0,0] yo = In[0,1] x = coor[0] y = coor[1] Xs = Ex[0,0] Ys = Ex[1,0] Zs = Ex[2,0] l_app[0,0] = (f*R[0,0]*Xs + f*R[1,0]*Ys + f*R[2,0]*Zs + (x-xo)*R[0,2]*Xs + (x-xo)*R[1,2]*Ys + (x-xo)*R[2,2]*Zs) l_app[1,0] = (f*R[0,1]*Xs + f*R[1,1]*Ys + f*R[2,1]*Zs + (y-yo)*R[0,2]*Xs + (y-yo)*R[1,2]*Ys + (y-yo)*R[2,2]*Zs) return l_app #main left_HomonymousImagePoints = [0.153,91.798] right_HomonymousImagePoints = [-78.672,89.122] left_In = np.mat([0,0,152.91]) left_Ex = np.mat([[970302.448784], [-1138644.971216], [3154.584941], [0.010425], [-0.012437], [0.003380]]) right_In = np.mat([0,0,152.91]) right_Ex = np.mat([[971265.303768], [-1138634.245942], [3154.784258], [0.008870], [-0.005062], [-0.008703]]) R_L = np.mat(np.zeros((3,3))) R_R = np.mat(np.zeros((3,3))) L = np.mat(np.zeros((4,3))) L_app = np.mat(np.zeros((4,1))) R_L = r_mat(left_Ex[3,0],left_Ex[4,0],left_Ex[5,0]) R_R = r_mat(right_Ex[3,0],right_Ex[4,0],right_Ex[5,0]) L[0:2,:] = l_mat(left_In,R_L,left_HomonymousImagePoints) L[2:4,:] = l_mat(right_In,R_R,right_HomonymousImagePoints) L_app[0:2,0] = l_approximate(left_In,R_L,left_HomonymousImagePoints,left_Ex) L_app[2:4,0] = l_approximate(right_In,R_R,right_HomonymousImagePoints,right_Ex) GPCoordinates = np.mat(np.zeros((3,1))) GPCoordinates = (L.T * L).I * L.T * L_app print("左影像同名点:",left_HomonymousImagePoints) print("左影像同名点:",right_HomonymousImagePoints) print("地面点坐标: X=%f, Y=%f, Z=%f" %(GPCoordinates[0,0],GPCoordinates[1,0],GPCoordinates[2,0]))