adjust.bas

摄影测量后方交会

Attribute VB_Name = "Adjust"
Option Explicit
Option Base 1
  Dim i As Integer
  Dim j As Integer
  Dim k As Integer
  Dim sum As Double
  Dim kk As Integer
Public Sub calc()
Dim r2(3, 3) As Double
    ReDim xx(g_KPotNum) As Double '像点坐标近似值
    Dim a1 As Double
    Dim a2 As Double
    Dim a3 As Double
    Dim b1 As Double
    Dim b2 As Double
    Dim b3 As Double
    Dim c1 As Double
    Dim c2 As Double
    Dim c3 As Double
    Dim cs1 As Double
    Dim cs2 As Double
    Dim cs3 As Double
    Dim s1 As Double
    Dim s2 As Double
    Dim s3 As Double
    Dim ii As Integer
    ReDim aa(2 * g_KPotNum, 6) As Double
    ReDim bb(6, 2 * g_KPotNum) As Double
    ReDim xx(2 * g_KPotNum) As Double
     ReDim yy(2 * g_KPotNum) As Double
     ReDim temp(2 * g_KPotNum) As Double
     Dim NN(21) As Double
    cs1 = Cos(g_A1)
    cs2 = Cos(g_A2)
    cs3 = Cos(g_A3)
    s1 = Sin(g_A1)
    s2 = Sin(g_A2)
    s3 = Sin(g_A3)
    
    a1 = cs1 * cs3 - s1 * s2 * s3
    a2 = -cs1 * s3 - s1 * s2 * cs3
    a3 = -s1 * cs2
    b1 = cs2 * s3
    b2 = cs2 * cs3
    b3 = -s2
    c1 = s1 * cs3 + cs1 * s2 * s3
    c2 = -s1 * s3 + cs1 * s2 * cs3
    c3 = cs1 * cs2
    
    r2(1, 1) = a1
    r2(1, 2) = a2
    r2(1, 3) = a3
    r2(2, 1) = b1
    r2(2, 2) = b2
    r2(2, 3) = b3
    r2(3, 1) = c1
    r2(3, 2) = c2
    r2(3, 3) = c3

  For ii = 1 To g_KPotNum
     
          aa(2 * ii - 1, 1) = -g_F / g_H
          aa(2 * ii - 1, 2) = 0
          aa(2 * ii - 1, 3) = -g_Lx(ii) / g_H
          aa(2 * ii - 1, 4) = -g_F * (1 + g_Lx(ii) ^ 2 / g_F ^ 2)
          aa(2 * ii - 1, 5) = -g_Lx(ii) * g_Ly(ii) / g_F
          aa(2 * ii - 1, 6) = g_Ly(ii)
          
          aa(2 * ii, 1) = 0
          aa(2 * ii, 2) = -g_F / g_H
          aa(2 * ii, 3) = -g_Ly(ii) / g_H
          aa(2 * ii, 4) = -g_Lx(ii) * g_Ly(ii) / g_F
          aa(2 * ii, 5) = -g_F * (1 + g_Ly(ii) ^ 2 / g_F ^ 2)
          aa(2 * ii, 6) = -g_Lx(ii)
    
       
      
       xx(ii) = -g_F * (a1 * (g_X(ii) - XS) + b1 * (g_Y(ii) - YS) + c1 * (g_Z(ii) - ZS)) / (a3 * (g_X(ii) - XS) + b3 * (g_Y(ii) - YS) + c3 * (g_Z(ii) - ZS))
       yy(ii) = -g_F * (a2 * (g_X(ii) - XS) + b2 * (g_Y(ii) - YS) + c2 * (g_Z(ii) - ZS)) / (a3 * (g_X(ii) - XS) + b3 * (g_Y(ii) - YS) + c3 * (g_Z(ii) - ZS))
       g_LL(2 * ii - 1) = g_Lx(ii) - xx(ii)
       g_LL(2 * ii) = g_Ly(ii) - yy(ii)
      
  Next ii
  
  
    For i = 1 To 6
        For j = 1 To 2 * g_KPotNum
          bb(i, j) = aa(j, i)
        Next j
      Next i
    For i = 1 To 6
          ii = (i - 1) * (2 * 5 - i) / 2
       For j = i To 6
           For k = 1 To 2 * g_KPotNum
            NN(ii + j) = NN(ii + j) + bb(i, k) * aa(k, j) '法方程一维存放
           Next k
       Next j
    Next i
   
   
 Call InVast(NN())
   
   For i = 1 To 6
     For j = 1 To 2 * g_KPotNum
          temp(i) = temp(i) + bb(i, j) * g_LL(j)
     Next j
   Next i
    For i = 1 To 6
       sum = 0
       ii = (i - 1) * (2 * 6 - i) / 2
       For j = i To 6
           sum = sum + NN(ii + j) * temp(j)
       Next j
       For k = 1 To i - 1
         kk = (k - 1) * (2 * 6 - k) / 2
         sum = sum + NN(kk + i) * temp(k)
       Next k
          g_XX(i) = sum
    Next i

End Sub
Sub InVast(a() As Double) '法方程求逆

  Dim p, g
  Dim m As Integer
  m = 6
  ReDim h(m) As Double
   Dim ii As Integer, jj As Integer
   For k = m To 1 Step -1
        p = a(1)
    For j = 2 To m
      h(j) = a(j)
    Next j
    ii = 0
    For i = 2 To m
      jj = ii
      ii = (i - 1) * (2 * m - i) / 2
      g = h(i) / p
     If i > k Then g = -g
      For j = i To m
        a(jj + j - 1) = a(ii + j) - h(j) * g
      Next j
      a(jj + m) = g
  Next i
      a(m * (m + 1) / 2) = 1 / p
    Next k
End Sub