Stereo pair of space intersection - collinear equation solution method (Python) implemented

First, the principle

 

 

 

 

Second, step

a. photograph is calculated by the respective left and right diagonal elements of a photograph of a rotation matrix R1 and R2.

B. dot same name listed collinearity equations.

c. Set the equation written in the form of a linear equation of unknowns, calculating the linear coefficient.

D. Write error equations, the coefficient matrix and the constant term.

E. unknown point least-squares solution is calculated.

F. calculate ground coordinates repeating the above steps for all points.

Third, the sample code

# -*- 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]))