API tools faq
paste
TheLegace

surveymath.py

TheLegace
May 11th, 2012
71
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 11.10 KB | None | 0 0
raw download clone embed print report
  1. """
  2. """
  3. import sys
  4. from constants import *
  5. from numpy import *
  6. from surveydata import *
  7.  
  8. class SurveyMath():
  9.    
  10.     def __init__(self):
  11.  
  12.         self.initEOP()     
  13.         self.initImagePoints()
  14.         self.EOP, self.EOPStd, self.sigma0 = self.spaceResection(self.gps, self.pix, affPara, f, ppxo, ppyo, self.EOPo_set, 0, 0, 0, 0, 0)
  15.         #~ self.EOPRight, self.EOPStdRight, self.sigma0Right = self.spaceResection(self.gps, self.pixR, affParaRight, fRight, ppxoRight,
  16.             #~ ppyoRight, self.EOPo_set_right, 0, 0, 0, 0, 0)
  17.         #~ self.spaceIntersection(self.gps, self.pixL, self.pixR, self.EOPLeft, self.EOPRight, leftIOP, rightIOP, affParaLeft, affParaRight)
  18.         #~ self.spaceIntersection(self.gps, self.pix
  19.         self.spaceIntersection(self.gps, self.pix, affPara, self.EOP, IOP)
  20.         #~ print self.EOPo_set_left
  21.         #~ print self.EOPo_set_right
  22.        
  23.     def initEOP(self):
  24.        
  25.         self.EOPo_set = array(p0_EOP, double)
  26.         #~ self.EOPo_set_right = array(p0_right_EOP, double)
  27.         #~ print self.EOPo_set_left[0:3]
  28.         #~ print self.EOPo_set_right[0:3]
  29.         self.EOPo_set[0:3] = self.EOPo_set[0:3] / 180 * pi
  30.         #~ self.EOPo_set_right[0:3] = self.EOPo_set_right[0:3] / 180 * pi
  31.         self.EOPo_set = self.EOPo_set[0:6]
  32.         #~ self.EOPo_set_right = self.EOPo_set_right[0:6]
  33.    
  34.     def initImagePoints(self):
  35.         fileImg = open('p0_leftCamA_imagePnts.txt', 'r')
  36.         #~ fileR = open('p0_rightCam_imagePnts.txt', 'r')
  37.         fileCtrl = open('petrieControl_NED.txt', 'r')
  38.        
  39.         pix = []
  40.         #~ pixR = []
  41.         gps = []
  42.         while 1:
  43.             line = fileImg.readline()
  44.             #~ lineR = fileR.readline()
  45.             lineCtrl = fileCtrl.readline()
  46.             if not lineCtrl:
  47.                 break          
  48.             pix.append([line.split()[1], line.split()[2]])
  49.             #~ pixR.append([lineR.split()[1], lineR.split()[2]])
  50.             gps.append([lineCtrl.split()[1], lineCtrl.split()[2],
  51.                 lineCtrl.split()[3]])
  52.         self.pix = array(pix, double)
  53.         #~ self.pixR = array(pixR, double)
  54.         self.gps = array(gps, double)
  55.        
  56.    
  57.     def spaceResection(self, gps, pix, affPara, f, ppx0, ppy0, EOPo, k1, k2, k3, p1, p2):
  58.         photoGCP = array([], double)
  59.         length = len(gps)
  60.         photoGCP.resize(pix.shape)
  61.         photoGCP[:,0:1] = (affPara[1]*pix[:,0:1])+(affPara[2]*pix[:,1:2]) + affPara[0]
  62.         photoGCP[:,1:2] = (affPara[4]*pix[:,0:1])+(affPara[5]*pix[:,1:2]) + affPara[3]
  63.         delta = EOPo.transpose()
  64.         tolerance = ones(len(delta), double) * 0.001
  65.         B2 = zeros((62,6), double)
  66.         epsilon = zeros(2*length, double)
  67.         Xnot = EOPo
  68.         count = 0
  69.         while count < 50:
  70.             row = 1
  71.             omega = Xnot[0]
  72.             phi = Xnot[1]
  73.             kappa = Xnot[2]
  74.             X1 = Xnot[3]
  75.             Y1 = Xnot[4]
  76.             Z1 = Xnot[5]
  77.             M = rotation3d(omega, phi, kappa)
  78.  
  79.            
  80.             i = 0
  81.             while i < length:
  82.                 r = (M[0,0]*(gps[i,0] - X1)) + (M[0,1]*(gps[i,1] - Y1)) + (M[0,2]*(gps[i,2] - Z1))
  83.                 s = (M[1,0]*(gps[i,0] - X1)) + (M[1,1]*(gps[i,1] - Y1)) + (M[1,2]*(gps[i,2] - Z1))
  84.                 q = (M[2,0]*(gps[i,0] - X1)) + (M[2,1]*(gps[i,1] - Y1)) + (M[2,2]*(gps[i,2] - Z1))
  85.                
  86.                 B2[(2*(i+1)-1)-1,0] = (f/(q**2))*((r*((-M[2,2]*(gps[i,1] - Y1))+
  87.                     (M[2,1]*(gps[i,2] - Z1))))-(q*((-M[0,2]*(gps[i,1] - Y1))+
  88.                     (M[0,1]*(gps[i,2] - Z1)))))
  89.                    
  90.                 B2[(2*(i+1)-1)-1,1] = (f/(q**2))*((r*((cos(phi)*(gps[i,0] - X1))+
  91.                     (sin(omega)*sin(phi)*(gps[i,1] - Y1)) - (cos(omega)*sin(phi)*
  92.                     (gps[i,2] - Z1))))-(q*((-sin(phi)*cos(kappa)*(gps[i,0] - X1))+
  93.                     (sin(omega)*cos(phi)*cos(kappa)*(gps[i,1] - Y1)) - (cos(omega)*
  94.                     cos(phi)*cos(kappa)*(gps[i,2] - Z1)))))
  95.                 B2[(2*(i+1)-1)-1,2] = (-f/q)*((M[1,0]*(gps[i,0] - X1))+(M[1,1]*
  96.                     (gps[i,1] - Y1))+(M[1,2]*(gps[i,2] - Z1)))
  97.                 B2[(2*(i+1)-1)-1,3] = -(f/(q**2))*((r*M[2,0]) - (q*M[0,0]))
  98.                 B2[(2*(i+1)-1)-1,4] = -(f/(q**2))*((r*M[2,1]) - (q*M[0,1]))
  99.                 B2[(2*(i+1)-1)-1,5] = -(f/(q**2))*((r*M[2,2]) - (q*M[0,2]))
  100.                
  101.                 B2[2*(i+1)-1,0] = (f/(q**2))* ((s*((-M[2,2]*(gps[i,1] - Y1))+(M[2,1]*
  102.                     (gps[i,2] - Z1))))-(q*((-M[1,2]*(gps[i,1] - Y1))+(M[1,1]*
  103.                     (gps[i,2] - Z1)))))
  104.                 B2[2*(i+1)-1,1] = (f/(q**2))*((s*((cos(phi)*(gps[i,0] - X1))+
  105.                     (sin(omega)*sin(phi)*(gps[i,1] - Y1)) - (cos(omega)*sin(phi)*
  106.                     (gps[i,2] - Z1))))-(q*((sin(phi)*sin(kappa)*(gps[i,0] - X1))-
  107.                     (sin(omega)*cos(phi)*sin(kappa)*(gps[i,1] - Y1)) + (cos(omega)*
  108.                     cos(phi)*sin(kappa)*(gps[i,2] - Z1)))))
  109.                 B2[2*(i+1)-1,2] = (f/q)*((M[0,0]*(gps[i,0] - X1))+(M[0,1]*(gps[i,1] - Y1))
  110.                     +(M[0,2]*(gps[i,2] - Z1)))
  111.                 B2[2*(i+1)-1,3] = -(f/(q**2))*((s*M[2,0]) - (q*M[1,0]))
  112.                 B2[2*(i+1)-1,4] = -(f/(q**2))*((s*M[2,1]) - (q*M[1,1]))
  113.                 B2[2*(i+1)-1,5] = -(f/(q**2))*((s*M[2,2]) - (q*M[1,2]))        
  114.                                
  115.                 rDist2 = ((photoGCP[i,0]**2)+(photoGCP[i,1]**2));
  116.                 deltaX = photoGCP[i,0]*rDist2*(k1+rDist2*(k2+rDist2*k3)) + p1*(rDist2+(2*photoGCP[i,0]**2)) + 2*p2*photoGCP[i,0]*photoGCP[i,1]
  117.                 deltaY = photoGCP[i,1]*rDist2*(k1+rDist2*(k2+rDist2*k3)) + p1*(rDist2+ (2*photoGCP[i,1]**2)) + 2*p2*photoGCP[i,0]*photoGCP[i,1]
  118.                
  119.                
  120.                 epsilon[(2*(i+1)-1)-1] = photoGCP[i,0] - ppx0 + (f*(r/q)) + deltaX
  121.                 epsilon[2*(i+1)-1] = photoGCP[i,1] - ppy0 + (f*(s/q)) + deltaY             
  122.                 i +=1
  123.            
  124.            
  125.             delta = dot(linalg.inv(dot(B2.transpose(), B2)), dot(B2.transpose(), epsilon.transpose()))
  126.             Xnot += delta
  127.             count += 1
  128.            
  129.         EOP = Xnot
  130.         sigma0 = dot(epsilon, epsilon.transpose())
  131.         EOPstd = sqrt(sigma0*(diag(linalg.inv(dot(B2.transpose(), B2)))))
  132.         return EOP, EOPstd, sigma0
  133.    
  134.    
  135.     def spaceIntersection(self, gps, pix, affPara, EOP, IOP):
  136.         X = EOP[3]
  137.         Y = EOP[4]
  138.         Z = EOP[5]
  139.  
  140.         M = rotation3d(EOP[0],EOP[1],EOP[2])
  141.  
  142.         f = IOP[0]
  143.         ppxo = IOP[1]
  144.         ppyo = IOP[2]
  145.         k1 = IOP[3]
  146.         k2 = IOP[4]
  147.         k3 = IOP[5]
  148.         p1 = IOP[6]
  149.         p2 = IOP[7]
  150.        
  151.         photo = array([], double)
  152.        
  153.         length = len(gps)
  154.         photo.resize(pix.shape)
  155.        
  156.         photo[:,0:1] = (affPara[1]*pix[:,0:1])+(affPara[2]*pix[:,1:2]) + affPara[0]
  157.         photo[:,1:2] = (affPara[4]*pix[:,0:1])+(affPara[5]*pix[:,1:2]) + affPara[3]    
  158.        
  159.         for i in range(0,20):
  160.             print i
  161.            
  162.            
  163.     #~ def spaceIntersection(self, gps, pixL, pixR, EOPleft, EOPright, IOPleft, IOPright, affParaLeft, affParaRight):
  164.         #~ X_left = EOPleft[3]
  165.         #~ Y_left = EOPleft[4]
  166.         #~ Z_left = EOPleft[5]
  167. #~
  168.         #~ X_right = EOPright[3]
  169.         #~ Y_right = EOPright[4]
  170.         #~ Z_right = EOPright[5]
  171.         #~
  172.         #~ M_left = rotation3d(EOPleft[0],EOPleft[1],EOPleft[2])
  173.         #~ M_right = rotation3d(EOPright[0],EOPright[1],EOPright[2])
  174.         #~
  175.         #~ fLeft = IOPleft[0]
  176.         #~ ppxoLeft = IOPleft[1]
  177.         #~ ppyoLeft = IOPleft[2]
  178.         #~ k1Left = IOPleft[3]
  179.         #~ k2Left = IOPleft[4]
  180.         #~ k3Left = IOPleft[5]
  181.         #~ p1Left = IOPleft[6]
  182.         #~ p2Left = IOPleft[7]
  183. #~
  184.         #~ fRight = IOPright[0]
  185.         #~ ppxoRight = IOPright[1]
  186.         #~ ppyoRight = IOPright[2]
  187.         #~ k1Right = IOPright[3]
  188.         #~ k2Right = IOPright[4]
  189.         #~ k3Right = IOPright[5]
  190.         #~ p1Right = IOPright[6]
  191.         #~ p2Right = IOPright[7]
  192.         #~
  193.         #~ photoLeft = array([], double)
  194.         #~ photoRight = array([], double)
  195.         #~
  196.         #~ length = len(gps)
  197.         #~ photoLeft.resize(pixL.shape)
  198.         #~ photoRight.resize(pixR.shape)
  199.         #~
  200.         #~ photoLeft[:,0:1] = (affParaLeft[1]*pixL[:,0:1])+(affParaLeft[2]*pixL[:,1:2]) + affParaLeft[0]
  201.         #~ photoLeft[:,1:2] = (affParaLeft[4]*pixL[:,0:1])+(affParaLeft[5]*pixL[:,1:2]) + affParaLeft[3]
  202.         #~
  203.         #~ photoRight[:,0:1] = (affParaRight[1]*pixR[:,0:1])+(affParaRight[2]*pixR[:,1:2]) + affParaRight[0]
  204.         #~ photoRight[:,1:2] = (affParaRight[4]*pixR[:,0:1])+(affParaRight[5]*pixR[:,1:2]) + affParaRight[3]       
  205.         #~
  206.         #~ B1 = zeros((length,2, 3), double)
  207.         #~ B2 = zeros((length,2, 3), double)
  208.         #~ B = zeros((length, 4, 3), double)
  209.         #~ epsilon1 = zeros((length, 2, 1))
  210.         #~ epsilon2 = zeros((length, 2, 1))
  211.         #~ epsilon = zeros((length, 4, 1))
  212.         #~
  213.          #~
  214.         #~ i = 0
  215.         #~ while i < 50:
  216.             #~ Brow = 1
  217.             #~ r1 = (M_left[0,0]*(gps[:,0:1] - X_left)) + (M_left[0,1]*(gps[:,1:2] - Y_left)) + (M_left[0,2]*(gps[:,2:3] - Z_left))
  218.             #~ s1 = (M_left[1,0]*(gps[:,0:1] - X_left)) + (M_left[1,1]*(gps[:,1:2] - Y_left)) + (M_left[1,2]*(gps[:,2:3] - Z_left))
  219.             #~ q1 = (M_left[2,0]*(gps[:,0:1] - X_left)) + (M_left[2,1]*(gps[:,1:2] - Y_left)) + (M_left[2,2]*(gps[:,2:3] - Z_left))
  220.             #~
  221.             #~ B1[:,2*Brow-1-1,0:1] = (fLeft/(q1**2))*((r1*M_left[2,0])-(q1*M_left[0,0]))
  222.             #~ B1[:,2*Brow-1-1,1:2]= (fLeft/(q1**2))*((r1*M_left[2,1])-(q1*M_left[0,1]))
  223.             #~ B1[:,2*Brow-1-1,2:3] = (fLeft/(q1**2))*((r1*M_left[2,2])-(q1*M_left[0,2]))
  224.             #~
  225.             #~ B1[:,2*Brow-1,0:1] = (fLeft/(q1**2))*((s1*M_left[2,0])-(q1*M_left[1,0]))
  226.             #~ B1[:,2*Brow-1,1:2] = (fLeft/(q1**2))*((s1*M_left[2,1])-(q1*M_left[1,1]))
  227.             #~ B1[:,2*Brow-1,2:3] = (fLeft/(q1**2))*((s1*M_left[2,2])-(q1*M_left[1,2]))
  228.             #~
  229.             #~
  230.             #~ rDist2 = ((photoLeft[:,0:1]**2)+(photoLeft[:,1:2]**2))
  231.             #~ deltaX = photoLeft[:,0:1]*rDist2*(k1Left+rDist2*(k2Left+rDist2*k3Left)) + p1Left*(rDist2+ (2*photoLeft[:,0:1]**2)) + 2*p2Left*photoLeft[:,0:1]*photoLeft[:,1:2]
  232.             #~ deltaY =  photoLeft[:,1:2]*rDist2*(k1Left+rDist2*(k2Left+rDist2*k3Left)) + p1Left*(rDist2+ (2*photoLeft[:,1:2]**2)) + 2*p2Left*photoLeft[:,0:1]*photoLeft[:,1:2]
  233.             #~
  234.             #~
  235.             #~ epsilon1[:,2*Brow-1-1,0:1] = photoLeft[:,0:1] - ppxoLeft + (fLeft*(r1/q1)) + deltaX #pnt(Brow,1) - ppx01 + (fLeft*(r1/q1));
  236.             #~ epsilon1[:,2*Brow-1,0:1] = photoLeft[:,1:2] - ppyoLeft + (fLeft*(s1/q1)) + deltaY #pnt(Brow,2) - ppy01 + (fLeft*(s1/q1));
  237.             #~
  238.             #~ r2 = (M_right[0,0]*(gps[:,0:1] - X_right)) + (M_right[0,1]*(gps[:,1:2] - Y_right)) + (M_right[0,2]*(gps[:,2:3] - Z_right))
  239.             #~ s2 = (M_right[1,0]*(gps[:,0:1] - X_right)) + (M_right[1,1]*(gps[:,1:2] - Y_right)) + (M_right[1,2]*(gps[:,2:3] - Z_right))
  240.             #~ q2 = (M_right[2,0]*(gps[:,0:1] - X_right)) + (M_right[2,1]*(gps[:,1:2] - Y_right)) + (M_right[2,2]*(gps[:,2:3] - Z_right))
  241. #~
  242.             #~ B2[:,2*Brow-1-1,0:1] = (fRight/(q1**2))*((r2*M_right[2,0])-(q2*M_right[0,0]))
  243.             #~ B2[:,2*Brow-1-1,1:2]= (fRight/(q1**2))*((r2*M_right[2,1])-(q2*M_right[0,1]))
  244.             #~ B2[:,2*Brow-1-1,2:3] = (fRight/(q1**2))*((r2*M_right[2,2])-(q2*M_right[0,2]))
  245.             #~
  246.             #~ B2[:,2*Brow-1,0:1] = (fRight/(q2**2))*((s2*M_right[2,0])-(q2*M_right[1,0]))
  247.             #~ B2[:,2*Brow-1,1:2] = (fRight/(q2**2))*((s2*M_right[2,1])-(q2*M_right[1,1]))
  248.             #~ B2[:,2*Brow-1,2:3] = (fRight/(q2**2))*((s2*M_right[2,2])-(q2*M_right[1,2]))
  249. #~
  250.             #~ rDist2 = ((photoRight[:,0:1]**2)+(photoRight[:,1:2]**2))
  251.             #~ deltaX = photoRight[:,0:1]*rDist2*(k1Right+rDist2*(k2Right+rDist2*k3Right)) + p1Right*(rDist2+ (2*photoRight[:,0:1]**2)) + 2*p2Right*photoRight[:,0:1]*photoRight[:,1:2]
  252.             #~ deltaY =  photoRight[:,1:2]*rDist2*(k1Right+rDist2*(k2Right+rDist2*k3Right)) + p1Right*(rDist2+ (2*photoRight[:,1:2]**2)) + 2*p2Right*photoRight[:,0:1]*photoRight[:,1:2]
  253.             #~
  254.             #~ epsilon2[:,2*Brow-1-1,0:1] = photoRight[:,0:1] - ppyoRight + (fRight*(r2/q2)) + deltaX #pnt(Brow,1) - ppx01 + (fLeft*(r1/q1));
  255.             #~ epsilon2[:,2*Brow-1,0:1] = photoRight[:,1:2] - ppyoRight + (fRight*(s2/q2)) + deltaY #pnt(Brow,2) - ppy01 + (fLeft*(s1/q1));
  256.             #~
  257.             #~
  258.             #~ B[:,0:2,:] = B1
  259.             #~ B[:,2:4,:] = B2
  260.             #~
  261.             #~ epsilon[:,0:2,:] = epsilon1
  262.             #~ epsilon[:,2:4,:] = epsilon2
  263.             #~ x = 0
  264.             #~ while x < length:
  265.                 #~ b = B[x]
  266.                 #~ e = epsilon[x]              
  267.                 #~ deltaX = dot(dot(linalg.inv(dot(b.T, b)), b.T), e)
  268.                 #~ gps[x] += deltaX
  269.                 #~
  270.                 #~ x +=1
  271.             #~
  272.                 #~
  273.             #~ print B.transpose()
  274.             #~ deltaX = dot(dot(linalg.inv(dot(B.transpose(), B)), B.transpose()), epsilon)
  275.             #~
  276.             #~ i += 1
  277.        
  278.        
  279.            
  280.        
  281. surveyMath = SurveyMath()
Advertisement
Add Comment
Please, Sign In to add comment
Public Pastes
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!