まず、原則
第二に、ステップ
。写真は、回転行列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("地面点坐标:\n X=%f,\n Y=%f,\n Z=%f" %(GPCoordinates[0,0],GPCoordinates[1,0],GPCoordinates[2,0]))