API tools faq
paste
TheLegace

surveymath.py

TheLegace
May 25th, 2012
52
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 15.27 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 = array([], double)
  15.         self.EOPStd = array([], double)
  16.         self.sigma0 = array([], double)
  17.        
  18.        
  19.         self.nPoints = len(self.pixs)
  20.         self.nImages = len(self.pixs[0])
  21.         #~ print self.pixs
  22.        
  23.         self.EOP.resize(self.nImages, 6)
  24.         self.EOPStd.resize(self.nImages, 6)
  25.         self.sigma0.resize(self.nImages, 1)
  26.        
  27.         for i in range(0, self.nImages):
  28.             pass
  29.             EOP, EOPStd, sigma0 = self.spaceResection(self.gps, self.pixs[:,i], affPara, f, ppxo, ppyo, self.EOPo_set, 0, 0, 0, 0, 0)
  30.             self.EOP[i,:] = EOP
  31.             self.EOPStd[i,:] = EOPStd
  32.             self.sigma0[i,:] = sigma0
  33.         #~ EOP, EOPStd, sigma0 = self.spaceResection(self.gps,\
  34.                 #~ self.pixs[:,1], array([-3.0656, 0.001681, 0.000000, 2.2979, 0.000000, -0.001680], double),\
  35.                     #~ f, ppxo, ppyo, self.EOPo_set, 0, 0, 0, 0, 0)
  36.         #~ self.EOP[1,:] = EOP
  37.         #~ self.EOPStd[1,:] = EOPStd
  38.         #~ self.sigma0[1,:] = sigma0                       
  39.        
  40.        
  41.         #~ self.
  42.         #~ for i in range(0,l):
  43.            
  44.             #~ self.EOP[:,i], self.EOPStd[:,i], self.sigma0[:,i] = self.spaceResection(self.gps,\
  45.                 #~ self.pixs[:,i], affPara, f, ppxo, ppyo, self.EOPo_set, 0, 0, 0, 0, 0)
  46.             #~ EOP, EOPStd, sigma0 = self.spaceResection(self.gps,\
  47.                 #~ self.pixs[:,i], affPara, f, ppxo, ppyo, self.EOPo_set, 0, 0, 0, 0, 0)
  48.             #~ print EOP
  49.             #~ self.EOP[i,:] = EOP
  50.             #~ self.EOPStd.append(EOPStd)
  51.             #~ self.sigma0.append(sigma0)
  52.         #~ print self.EOP[:,1]
  53.        
  54.         #~ for i in range(0,l):
  55.             #~
  56.            
  57.        
  58.        
  59.         for i in range(0, 1):
  60.             self.spaceIntersection(self.pixs[i], affPara, self.EOP, IOP)
  61.             #~ print self.pixs[i][0]
  62.        
  63.        
  64.         #~ self.EOPRight, self.EOPStdRight, self.sigma0Right = self.spaceResection(self.gps, self.pixR, affParaRight, fRight, ppxoRight,
  65.             #~ ppyoRight, self.EOPo_set_right, 0, 0, 0, 0, 0)
  66.         #~ self.spaceIntersection(self.gps, self.pixL, self.pixR, self.EOPLeft, self.EOPRight, leftIOP, rightIOP, affParaLeft, affParaRight)
  67.         #~ self.spaceIntersection(self.gps, self.pix
  68.         #~ self.spaceIntersection(self.gps, self.pix, affPara, self.EOP, IOP)
  69.         #~ print self.EOPo_set_left
  70.         #~ print self.EOPo_set_right
  71.        
  72.     def initEOP(self):
  73.        
  74.         self.EOPo_set = array(p0_EOP, double)
  75.         #~ self.EOPo_set_right = array(p0_right_EOP, double)
  76.         #~ print self.EOPo_set_left[0:3]
  77.         #~ print self.EOPo_set_right[0:3]
  78.         self.EOPo_set[0:3] = self.EOPo_set[0:3] / 180 * pi
  79.         #~ self.EOPo_set_right[0:3] = self.EOPo_set_right[0:3] / 180 * pi
  80.         self.EOPo_set = self.EOPo_set[0:6]
  81.         #~ self.EOPo_set_right = self.EOPo_set_right[0:6]
  82.    
  83.     def initImagePoints(self):
  84.         fileImg1 = open('p0_leftCamA_imagePnts.txt', 'r')
  85.         fileImg2 = open('p0_rightCam_imagePnts.txt', 'r')
  86.         fileCtrl = open('petrieControl_NED.txt', 'r')
  87.        
  88.         pixL = []
  89.         pixR = []
  90.         gps = []
  91.         i = 0
  92.         self.pixs = zeros((31, 2, 2), double)
  93.        
  94.         while 1:
  95.             line1 = fileImg1.readline()
  96.             line2 = fileImg2.readline()
  97.             #~ lineR = fileR.readline()
  98.             lineCtrl = fileCtrl.readline()
  99.             if not lineCtrl:
  100.                 break          
  101.             #~ pixL.append(array([[line1.split()[1], line1.split()[2]],[line2.split()[1], line2.split()[2]]], double))
  102.             #~ pixR.append([lineR.split()[1], lineR.split()[2]])
  103.             self.pixs[i] = array([[line1.split()[1], line1.split()[2]], [line2.split()[1], line2.split()[2]]], double)
  104.             #~ pixL.append(array([line1.split()[1], line1.split()[2]], double))
  105.             #~ pixR.append(array([line2.split()[1], line2.split()[2]], double))
  106.             gps.append([lineCtrl.split()[1], lineCtrl.split()[2],
  107.                 lineCtrl.split()[3]])
  108.             i += 1
  109.         #~ self.pixs = array([pixL, pixR], double)
  110.         #~ self.pixR = array(pixR, double)
  111.         self.gps = array(gps, double)
  112.        
  113.    
  114.     def spaceResection(self, gps, pix, affPara, f, ppx0, ppy0, EOPo, k1, k2, k3, p1, p2):  
  115.         photoGCP = array([], double)
  116.         length = len(gps)
  117.         photoGCP.resize(pix.shape)
  118.         photoGCP[:,0:1] = (affPara[1]*pix[:,0:1])+(affPara[2]*pix[:,1:2]) + affPara[0]
  119.         photoGCP[:,1:2] = (affPara[4]*pix[:,0:1])+(affPara[5]*pix[:,1:2]) + affPara[3]
  120.         delta = EOPo.transpose()
  121.         tolerance = ones(len(delta), double) * 0.001
  122.         B2 = zeros((62,6), double)
  123.         epsilon = zeros(2*length, double)
  124.         Xnot = EOPo
  125.         count = 0
  126.         while count < 50:
  127.             row = 1
  128.             omega = Xnot[0]
  129.             phi = Xnot[1]
  130.             kappa = Xnot[2]
  131.             X1 = Xnot[3]
  132.             Y1 = Xnot[4]
  133.             Z1 = Xnot[5]
  134.             M = rotation3d(omega, phi, kappa)
  135.  
  136.            
  137.             i = 0
  138.             while i < length:
  139.                 r = (M[0,0]*(gps[i,0] - X1)) + (M[0,1]*(gps[i,1] - Y1)) + (M[0,2]*(gps[i,2] - Z1))
  140.                 s = (M[1,0]*(gps[i,0] - X1)) + (M[1,1]*(gps[i,1] - Y1)) + (M[1,2]*(gps[i,2] - Z1))
  141.                 q = (M[2,0]*(gps[i,0] - X1)) + (M[2,1]*(gps[i,1] - Y1)) + (M[2,2]*(gps[i,2] - Z1))
  142.                
  143.                 B2[(2*(i+1)-1)-1,0] = (f/(q**2))*((r*((-M[2,2]*(gps[i,1] - Y1))+
  144.                     (M[2,1]*(gps[i,2] - Z1))))-(q*((-M[0,2]*(gps[i,1] - Y1))+
  145.                     (M[0,1]*(gps[i,2] - Z1)))))
  146.                    
  147.                 B2[(2*(i+1)-1)-1,1] = (f/(q**2))*((r*((cos(phi)*(gps[i,0] - X1))+
  148.                     (sin(omega)*sin(phi)*(gps[i,1] - Y1)) - (cos(omega)*sin(phi)*
  149.                     (gps[i,2] - Z1))))-(q*((-sin(phi)*cos(kappa)*(gps[i,0] - X1))+
  150.                     (sin(omega)*cos(phi)*cos(kappa)*(gps[i,1] - Y1)) - (cos(omega)*
  151.                     cos(phi)*cos(kappa)*(gps[i,2] - Z1)))))
  152.                 B2[(2*(i+1)-1)-1,2] = (-f/q)*((M[1,0]*(gps[i,0] - X1))+(M[1,1]*
  153.                     (gps[i,1] - Y1))+(M[1,2]*(gps[i,2] - Z1)))
  154.                 B2[(2*(i+1)-1)-1,3] = -(f/(q**2))*((r*M[2,0]) - (q*M[0,0]))
  155.                 B2[(2*(i+1)-1)-1,4] = -(f/(q**2))*((r*M[2,1]) - (q*M[0,1]))
  156.                 B2[(2*(i+1)-1)-1,5] = -(f/(q**2))*((r*M[2,2]) - (q*M[0,2]))
  157.                
  158.                 B2[2*(i+1)-1,0] = (f/(q**2))* ((s*((-M[2,2]*(gps[i,1] - Y1))+(M[2,1]*
  159.                     (gps[i,2] - Z1))))-(q*((-M[1,2]*(gps[i,1] - Y1))+(M[1,1]*
  160.                     (gps[i,2] - Z1)))))
  161.                 B2[2*(i+1)-1,1] = (f/(q**2))*((s*((cos(phi)*(gps[i,0] - X1))+
  162.                     (sin(omega)*sin(phi)*(gps[i,1] - Y1)) - (cos(omega)*sin(phi)*
  163.                     (gps[i,2] - Z1))))-(q*((sin(phi)*sin(kappa)*(gps[i,0] - X1))-
  164.                     (sin(omega)*cos(phi)*sin(kappa)*(gps[i,1] - Y1)) + (cos(omega)*
  165.                     cos(phi)*sin(kappa)*(gps[i,2] - Z1)))))
  166.                 B2[2*(i+1)-1,2] = (f/q)*((M[0,0]*(gps[i,0] - X1))+(M[0,1]*(gps[i,1] - Y1))
  167.                     +(M[0,2]*(gps[i,2] - Z1)))
  168.                 B2[2*(i+1)-1,3] = -(f/(q**2))*((s*M[2,0]) - (q*M[1,0]))
  169.                 B2[2*(i+1)-1,4] = -(f/(q**2))*((s*M[2,1]) - (q*M[1,1]))
  170.                 B2[2*(i+1)-1,5] = -(f/(q**2))*((s*M[2,2]) - (q*M[1,2]))        
  171.                                
  172.                 rDist2 = ((photoGCP[i,0]**2)+(photoGCP[i,1]**2));
  173.                 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]
  174.                 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]
  175.                
  176.                
  177.                 epsilon[(2*(i+1)-1)-1] = photoGCP[i,0] - ppx0 + (f*(r/q)) + deltaX
  178.                 epsilon[2*(i+1)-1] = photoGCP[i,1] - ppy0 + (f*(s/q)) + deltaY             
  179.                 i +=1
  180.            
  181.            
  182.             delta = dot(linalg.inv(dot(B2.transpose(), B2)), dot(B2.transpose(), epsilon.transpose()))
  183.             Xnot += delta
  184.             count += 1
  185.            
  186.         EOP = Xnot
  187.         sigma0 = dot(epsilon, epsilon.transpose())
  188.         EOPstd = sqrt(sigma0*(diag(linalg.inv(dot(B2.transpose(), B2)))))
  189.         return EOP, EOPstd, sigma0
  190.    
  191.    
  192.     def spaceIntersection(self, pixs, affPara, EOP, IOP):
  193.        
  194.         X = array(EOP[:,3:4], double)
  195.         Y = array(EOP[:,4:5], double)
  196.         Z = array(EOP[:,5:6], double)      
  197.        
  198.         M = array([], double)
  199.         M.resize(self.nImages, 3, 3, 1)
  200.        
  201.        
  202.        
  203.         #~ for i in range(0,l):
  204.             #~ M[i] = rotation3d(EOP[0],EOP[1],EOP[2])
  205.         #~ M = rotation3d(EOP[0],EOP[1],EOP[2])
  206.  
  207.         f = IOP[0]
  208.         ppxo = IOP[1]
  209.         ppyo = IOP[2]
  210.         k1 = IOP[3]
  211.         k2 = IOP[4]
  212.         k3 = IOP[5]
  213.         p1 = IOP[6]
  214.         p2 = IOP[7]
  215.                
  216.         photo = array([], double)
  217.        
  218.         photo.resize(self.nImages, self.nPoints, 2)
  219.         #~ print photo.shape
  220.        
  221.        
  222.         #~ for i in range(0,self.nImages):         
  223.             #~ print M[i,:]
  224.             #~ print rotation3d(EOP[i,0:1], EOP[i,1:2], EOP[i,2:3])
  225.         #~ print EOP[0,0:1]
  226.         #~ print EOP[5,0:1]
  227.        
  228.         for i in range(0, self.nImages):
  229.             M[i] = rotation3d(EOP[i,0:1], EOP[i,1:2], EOP[i,2:3])
  230.             #~ print str(pixs[i,:,0:1]) + '\n'
  231.             photo[i,:,0:1] = (affPara[1]*pixs[i,:,0:1])+(affPara[2]*pixs[i,:,1:2])+affPara[0]
  232.             photo[i,:,1:2] = (affPara[4]*pixs[i,:,0:1])+(affPara[5]*pixs[i,:,1:2])+affPara[3]
  233.             #~ print M[i]
  234.             #~ M[i] =
  235.             #~ photo[i,:,0:1] = (affPara[1]*pixs[i,:,0:1])+(affPara[2]*pixs[i,:,1:2]) + affPara[0]
  236.             #~ print str(pixs[i,:][:]) + '\n'
  237.         #~ print M
  238.        
  239.         X0 = array([32.5600, -12.7000,  -1.2500], double)
  240.  
  241.         M.resize(self.nImages, 3, 3)
  242.        
  243.         B1 = zeros((2, 3), double)
  244.         epsilon1 = zeros((self.nPoints, 2, 1))
  245.         epsilon = zeros((self.nPoints, 4, 1))
  246.        
  247.         #~ for itera in range(0, 20):
  248.             #~ for i in range(0, self.nImages):
  249.         for itera in range(0, 20):
  250.             for i in range(0, 1):          
  251.                 Brow = 0
  252.                
  253.                 r1 = (M[i,0,0]*(X0[0] - X[i])) + (M[i,0,1]*(X0[1] - Y[i])) + (M[i,0,2]*(X0[2] - Z[i]))
  254.                 s1 = (M[i,1,0]*(X0[0] - X[i])) + (M[i,1,1]*(X0[1] - Y[i])) + (M[i,1,2]*(X0[2] - Z[i]))
  255.                 q1 = (M[i,2,0]*(X0[0] - X[i])) + (M[i,2,1]*(X0[1] - Y[i])) + (M[i,2,2]*(X0[2] - Z[i]))
  256.                 #~ B1[2*(i+1)-1-1][0][0]
  257.                
  258.                 B1[0][0] = (f/(q1**2))*((r1*M[i,2,0])-(q1*M[i,0,0]))
  259.                 B1[0][1] = (f/(q1**2))*((r1*M[i,2,1])-(q1*M[i,0,1]))
  260.                 B1[0][2] = (f/(q1**2))*((r1*M[i,2,2])-(q1*M[i,0,2]))
  261.                
  262.                
  263.                 B1[1][0] = (f/(q1**2))*((s1*M[i,2,0])-(q1*M[i,1,0]))
  264.                 B1[1][1] = (f/(q1**2))*((s1*M[i,2,1])-(q1*M[i,1,1]))
  265.                 B1[1][2] = (f/(q1**2))*((s1*M[i,2,2])-(q1*M[i,1,2]))
  266.                 #~ print B1
  267.                
  268.                 if i == 0:
  269.                     B = B1
  270.                
  271.                 rDist2 = ((photo[i,0:1,0]**2)+(photo[i,0:1,1]**2))
  272.                 deltaX = photo[i,0:1,0]*rDist2*(k1+rDist2*(k2+rDist2*k3)) + p1*(rDist2+ (2*photo[i,0:1,0]**2)) + 2*p2*photo[i,0:1,0]*photo[i,0:1,1]
  273.                 deltaY =  photo[i,0:1,1]*rDist2*(k1+rDist2*(k2+rDist2*k3)) + p1*(rDist2+ (2*photo[i,0:1,1]**2)) + 2*p2*photo[i,0:1,0]*photo[i,0:1,1]   
  274.                
  275.                
  276.                
  277.                 epsilon1[i,2*Brow-1-1,0:1] = photo[i,0:1,0] - ppxo + (f*(r1/q1)) + deltaX #pnt(Brow,1) - ppx01 + (fLeft*(r1/q1));
  278.                 epsilon1[i,2*Brow-1,0:1] = photo[i,0:1,1] - ppyo + (f*(s1/q1)) + deltaY #pnt(Brow,2) - ppy01 + (fLeft*(s1/q1));
  279.                
  280.                 Brow = Brow + 1
  281.                
  282.        
  283.        
  284.         #~ photo[:,0:1] = (affPara[1]*pix[:,0:1])+(affPara[2]*pix[:,1:2]) + affPara[0]
  285.         #~ photo[:,1:2] = (affPara[4]*pix[:,0:1])+(affPara[5]*pix[:,1:2]) + affPara[3]     
  286.         #~
  287.         #~ for i in range(0,20):
  288.             #~ print i
  289.            
  290.            
  291.     #~ def spaceIntersection(self, gps, pixL, pixR, EOPleft, EOPright, IOPleft, IOPright, affParaLeft, affParaRight):
  292.         #~ X = EOPleft[3]
  293.         #~ Y = EOPleft[4]
  294.         #~ Z = EOPleft[5]
  295. #~
  296.         #~ X_right = EOPright[3]
  297.         #~ Y_right = EOPright[4]
  298.         #~ Z_right = EOPright[5]
  299.         #~
  300.         #~ M = rotation3d(EOPleft[0],EOPleft[1],EOPleft[2])
  301.         #~ M_right = rotation3d(EOPright[0],EOPright[1],EOPright[2])
  302.         #~
  303.         #~ fLeft = IOPleft[0]
  304.         #~ ppxoLeft = IOPleft[1]
  305.         #~ ppyoLeft = IOPleft[2]
  306.         #~ k1Left = IOPleft[3]
  307.         #~ k2Left = IOPleft[4]
  308.         #~ k3Left = IOPleft[5]
  309.         #~ p1Left = IOPleft[6]
  310.         #~ p2Left = IOPleft[7]
  311. #~
  312.         #~ fRight = IOPright[0]
  313.         #~ ppxoRight = IOPright[1]
  314.         #~ ppyoRight = IOPright[2]
  315.         #~ k1Right = IOPright[3]
  316.         #~ k2Right = IOPright[4]
  317.         #~ k3Right = IOPright[5]
  318.         #~ p1Right = IOPright[6]
  319.         #~ p2Right = IOPright[7]
  320.         #~
  321.         #~ photoLeft = array([], double)
  322.         #~ photoRight = array([], double)
  323.         #~
  324.         #~ length = len(gps)
  325.         #~ photoLeft.resize(pixL.shape)
  326.         #~ photoRight.resize(pixR.shape)
  327.         #~
  328.         #~ photoLeft[:,0:1] = (affParaLeft[1]*pixL[:,0:1])+(affParaLeft[2]*pixL[:,1:2]) + affParaLeft[0]
  329.         #~ photoLeft[:,1:2] = (affParaLeft[4]*pixL[:,0:1])+(affParaLeft[5]*pixL[:,1:2]) + affParaLeft[3]
  330.         #~
  331.         #~ photoRight[:,0:1] = (affParaRight[1]*pixR[:,0:1])+(affParaRight[2]*pixR[:,1:2]) + affParaRight[0]
  332.         #~ photoRight[:,1:2] = (affParaRight[4]*pixR[:,0:1])+(affParaRight[5]*pixR[:,1:2]) + affParaRight[3]       
  333.         #~
  334.         #~ B1 = zeros((length,2, 3), double)
  335.         #~ B2 = zeros((length,2, 3), double)
  336.         #~ B = zeros((length, 4, 3), double)
  337.         #~ epsilon1 = zeros((length, 2, 1))
  338.         #~ epsilon2 = zeros((length, 2, 1))
  339.         #~ epsilon = zeros((length, 4, 1))
  340.         #~
  341.          #~
  342.         #~ i = 0
  343.         #~ while i < 50:
  344.             #~ Brow = 1
  345.             #~ r1 = (M[0,0]*(gps[:,0:1] - X)) + (M[0,1]*(gps[:,1:2] - Y)) + (M[0,2]*(gps[:,2:3] - Z))
  346.             #~ s1 = (M[1,0]*(gps[:,0:1] - X)) + (M[1,1]*(gps[:,1:2] - Y)) + (M[1,2]*(gps[:,2:3] - Z))
  347.             #~ q1 = (M[2,0]*(gps[:,0:1] - X)) + (M[2,1]*(gps[:,1:2] - Y)) + (M[2,2]*(gps[:,2:3] - Z))
  348.             #~
  349.             #~ B1[:,2*Brow-1-1,0:1] = (fLeft/(q1**2))*((r1*M[2,0])-(q1*M[0,0]))
  350.             #~ B1[:,2*Brow-1-1,1:2]= (fLeft/(q1**2))*((r1*M[2,1])-(q1*M[0,1]))
  351.             #~ B1[:,2*Brow-1-1,2:3] = (fLeft/(q1**2))*((r1*M[2,2])-(q1*M[0,2]))
  352.             #~
  353.             #~ B1[:,2*Brow-1,0:1] = (fLeft/(q1**2))*((s1*M[2,0])-(q1*M[1,0]))
  354.             #~ B1[:,2*Brow-1,1:2] = (fLeft/(q1**2))*((s1*M[2,1])-(q1*M[1,1]))
  355.             #~ B1[:,2*Brow-1,2:3] = (fLeft/(q1**2))*((s1*M[2,2])-(q1*M[1,2]))
  356.             #~
  357.             #~
  358.             #~ rDist2 = ((photoLeft[:,0:1]**2)+(photoLeft[:,1:2]**2))
  359.             #~ 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]
  360.             #~ 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]
  361.             #~
  362.             #~
  363.             #~ epsilon1[:,2*Brow-1-1,0:1] = photoLeft[:,0:1] - ppxoLeft + (fLeft*(r1/q1)) + deltaX #pnt(Brow,1) - ppx01 + (fLeft*(r1/q1));
  364.             #~ epsilon1[:,2*Brow-1,0:1] = photoLeft[:,1:2] - ppyoLeft + (fLeft*(s1/q1)) + deltaY #pnt(Brow,2) - ppy01 + (fLeft*(s1/q1));
  365.             #~
  366.             #~ 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))
  367.             #~ 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))
  368.             #~ 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))
  369. #~
  370.             #~ B2[:,2*Brow-1-1,0:1] = (fRight/(q1**2))*((r2*M_right[2,0])-(q2*M_right[0,0]))
  371.             #~ B2[:,2*Brow-1-1,1:2]= (fRight/(q1**2))*((r2*M_right[2,1])-(q2*M_right[0,1]))
  372.             #~ B2[:,2*Brow-1-1,2:3] = (fRight/(q1**2))*((r2*M_right[2,2])-(q2*M_right[0,2]))
  373.             #~
  374.             #~ B2[:,2*Brow-1,0:1] = (fRight/(q2**2))*((s2*M_right[2,0])-(q2*M_right[1,0]))
  375.             #~ B2[:,2*Brow-1,1:2] = (fRight/(q2**2))*((s2*M_right[2,1])-(q2*M_right[1,1]))
  376.             #~ B2[:,2*Brow-1,2:3] = (fRight/(q2**2))*((s2*M_right[2,2])-(q2*M_right[1,2]))
  377. #~
  378.             #~ rDist2 = ((photoRight[:,0:1]**2)+(photoRight[:,1:2]**2))
  379.             #~ 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]
  380.             #~ 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]
  381.             #~
  382.             #~ epsilon2[:,2*Brow-1-1,0:1] = photoRight[:,0:1] - ppyoRight + (fRight*(r2/q2)) + deltaX #pnt(Brow,1) - ppx01 + (fLeft*(r1/q1));
  383.             #~ epsilon2[:,2*Brow-1,0:1] = photoRight[:,1:2] - ppyoRight + (fRight*(s2/q2)) + deltaY #pnt(Brow,2) - ppy01 + (fLeft*(s1/q1));
  384.             #~
  385.             #~
  386.            
  387.             #~ B[:,2:4,:] = B2
  388.             #~
  389.             #~ epsilon[:,0:2,:] = epsilon1
  390.             #~ epsilon[:,2:4,:] = epsilon2
  391.             #~ x = 0
  392.             #~ while x < length:
  393.                 #~ b = B[x]
  394.                 #~ e = epsilon[x]              
  395.                 #~ deltaX = dot(dot(linalg.inv(dot(b.T, b)), b.T), e)
  396.                 #~ gps[x] += deltaX
  397.                 #~
  398.                 #~ x +=1
  399.             #~
  400.                 #~
  401.             #~ print B.transpose()
  402.             #~ deltaX = dot(dot(linalg.inv(dot(B.transpose(), B)), B.transpose()), epsilon)
  403.             #~
  404.             #~ i += 1
  405. 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!