摄影测量学空间后方交会Python代码

引言

这是山东科技大学测绘学院遥感专业18级大三课程王志勇《摄影测量学》的编程家庭作业。

输入给定，使用Python代码求解。输入情况在代码底部。

代码构建了一个空间后方交会类，没有经过任何优化，赶出来的代码，很多地方写的很丑陋很暴力，但是的确能够工作。

代码

"""
后方交会代码
作者：Dash
version:0.1
without any optimization
"""

import numpy as np

class RearIntersection(object):
    def __init__(self,f, X, Y, Z, x, y, m = 50000):
        """
        `X`,`Y`,`Z`,`x`,`y` is list of control point paras.
        """

        # transform to np arrays.
        self._f = np.array(f, dtype = np.float32)
        # control point
        self._X = np.array(X, dtype = np.float32)
        self._Y = np.array(Y, dtype = np.float32)
        self._Z = np.array(Z, dtype = np.float32)
        # image point
        self._x = np.array(x, dtype = np.float32)
        self._y = np.array(y, dtype = np.float32)
        # scale
        self._m = np.array(m, dtype = np.float32)

        self._H = self._m * self._f

    def solve(self):
        
        phi = 0
        omega = 0
        kappa = 0

        n = len(self._x)
        
        # initial line paras
        Z0 = self._m * self._f + (1/n) * np.sum(self._Z)
        X0 = (1/n) * np.sum(self._X)
        Y0 = (1/n) * np.sum(self._Y)

        Max_Step = 100000
        Xt = X0
        Yt = Y0
        Zt = Z0
        
        for i in range(Max_Step):

            a1 = np.cos(phi)*np.cos(kappa)-np.sin(phi)*np.sin(omega)*np.sin(kappa)
            a2 = -np.cos(phi)*np.sin(kappa)-np.sin(phi)*np.sin(omega)*np.cos(kappa)
            a3 = -np.sin(phi)*np.cos(omega)
            b1 = np.cos(omega)*np.sin(kappa)
            b2 = np.cos(omega)*np.cos(kappa)
            b3 = -np.sin(omega)
            c1 = np.sin(phi)*np.cos(kappa)+np.cos(phi)*np.sin(omega)*np.sin(kappa)
            c2 = -np.sin(phi)*np.sin(kappa)+np.cos(phi)*np.sin(omega)*np.cos(kappa)
            c3 = np.cos(phi)*np.cos(omega)

            rotation_matrix = np.array([[a1,a2,a3], [b1, b2, b3], [c1, c2, c3]])

            L = []
            for j in range(len(self._x)):
                numerator_x = self._f*(a1*(self._X[j]-Xt)+b1*(self._Y[j] - Yt)+c1*(self._Z[j] - Zt))
                numerator_y = self._f*(a2*(self._X[j]-Xt)+b2*(self._Y[j] - Yt)+c2*(self._Z[j] - Zt))
                denomanator = (a3*(self._X[j]-Xt)+b3*(self._Y[j] - Yt)+c3*(self._Z[j] - Zt))
                lx = self._x[j] + numerator_x/denomanator
                ly = self._y[j] + numerator_y/denomanator
                L.append(lx)
                L.append(ly)
            L = np.array(L)
            num_of_samples = len(self._x)

            a11 = np.zeros(num_of_samples, dtype = np.float32)
            b11 = np.zeros(num_of_samples, dtype = np.float32)
            c11 = np.zeros(num_of_samples, dtype = np.float32)
            d11 = np.zeros(num_of_samples, dtype = np.float32)
            e11 = np.zeros(num_of_samples, dtype = np.float32)
            f11 = np.zeros(num_of_samples, dtype = np.float32)
            
            a22 = np.zeros(num_of_samples, dtype = np.float32)
            b22 = np.zeros(num_of_samples, dtype = np.float32)
            c22 = np.zeros(num_of_samples, dtype = np.float32)
            d22 = np.zeros(num_of_samples, dtype = np.float32)
            e22 = np.zeros(num_of_samples, dtype = np.float32)
            f22 = np.zeros(num_of_samples, dtype = np.float32)

            for j in range(num_of_samples):
                z_bar = a3*(self._X[j]-Xt)+b3*(self._Y[j]-Yt)+c3*(self._Z[j]-Zt)
                a11[j] = (a1*self._f + a3*self._x[j])/z_bar
                b11[j] = (b1*self._f+b3*self._x[j])/z_bar
                c11[j] = (c1*self._f+c3*self._x[j])/z_bar
                d11[j] = (self._y[j]*np.sin(omega))-(self._x[j]*(self._x[j]*np.cos(kappa)-self._y[j]*np.sin(kappa))/self._f+self._f*np.cos(kappa))*np.cos(omega)
                e11[j] = -self._f*np.sin(kappa)-(self._x[j]/self._f)*(self._x[j]*np.sin(kappa)+self._y[j]*np.cos(kappa))
                f11[j] = self._y[j]
                
                a22[j] = (a2*self._f + a3*self._y[j])/z_bar
                b22[j] = (b2*self._f + b3*self._y[j])/z_bar
                c22[j] = (c2*self._f + c3*self._y[j])/z_bar
                d22[j] = -self._x[j]*np.sin(omega) - (self._y[j]/self._f)*((self._x[j]*np.cos(kappa)-self._y[j]*np.sin(kappa))-self._f*np.sin(kappa))*np.cos(omega)
                e22[j] = -self._f*np.cos(kappa)-(self._y[j]/self._f)*(self._x[j]*np.sin(kappa)+self._y[j]*np.cos(kappa))
                f22[j] = -self._x[j]



            A = np.zeros((2*num_of_samples, 6), dtype = np.float32)
            for j in range(num_of_samples):
                 A[2*j:2*j+2, :] = np.array([[a11[j], b11[j], c11[j], d11[j], e11[j], f11[j]], [a22[j], b22[j], c22[j], d22[j], e22[j], f22[j]]], dtype = np.float32)

            AtA = np.dot(A.T, A)
            inverse_AtA = np.linalg.inv(AtA)
            At = A.T
            combine = np.dot(inverse_AtA, A.T)
            X = np.dot(combine, L)

            dXs = X[0]
            dYs = X[1]
            dZs = X[2]
            dphi = X[3]
            domega = X[4]
            dkappa = X[5]

            Xt += dXs
            Yt += dYs
            Zt += dZs
            phi += dphi
            omega += domega
            kappa += dkappa

            limit = 0.00001
            if np.abs(dphi) < limit or np.abs(domega) < limit or np.abs(dkappa) < limit:
                return (Xt, Yt, Zt, phi, omega, kappa)

def main():
    f = 153.24 / 1000
    x = [-86.15/1000, -53.40/1000, -14.78/1000, 10.46/1000]
    y = [-68.99/1000, 82.21/1000, -76.63/1000, 64.43/1000]
    X = [36589.41, 37631.08, 39100.97, 40426.54]
    Y = [25273.32, 31324.51, 24934.98, 30319.81]
    Z = [2195.17, 728.69, 2386.50, 757.31]
    
    rear_intersection = RearIntersection(f, X, Y, Z, x, y)
    X = rear_intersection.solve()
    print(X)

if __name__ == '__main__':
    main()