立体像对空间前方交会-共线方程求解法（python实现）

一、原理

二、步骤

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("地面点坐标：\n         X=%f,\n         Y=%f,\n         Z=%f"
      %(GPCoordinates[0,0],GPCoordinates[1,0],GPCoordinates[2,0]))