module1.bas

用于摄影测量航带网概算的程序

Attribute VB_Name = "Module1"
   Public name1() As Integer        '第一像对中的点号
   Public name2() As Integer        '第一像对中的点号
   Public name3() As Integer        '控制点点号
   Public f As Double               '焦距
   Public bx As Double              '摄影基线
   Public e As Double               '迭代限差
   Public x11() As Double           '第一像对中左片同名点坐标
   Public y11() As Double
   Public x12() As Double           '第一像对中右片同名点坐标
   Public y12() As Double
   Public x21() As Double           '第二像对中左片同名点坐标
   Public y21() As Double
   Public x22() As Double           '第二像对中右片同名点坐标
   Public y22() As Double
   Public xkong() As Double         '控制点的坐标
   Public ykong() As Double
   Public zkong() As Double
   Public num(1 To 3)  As Integer   '第一,二像对中观测点个,数控制点个数
   '像对的相对定向元素
   Public fy(1 To 2)  As Double
   Public om(1 To 2) As Double
   Public kp(1 To 2) As Double
   Public u(1 To 2) As Double
   Public V(1 To 2) As Double
   '保存像对的投影系数
   Public N11() As Double
   Public N12() As Double
   Public N21() As Double
   Public N22() As Double
   '保存基线向量
   Public bysave(1 To 2)
   Public bzsave(1 To 2)
   '模型点坐标
   Public Xmo() As Double
   Public Ymo() As Double
   Public Zmo() As Double
   '同名象点的象空间辅助坐标
   Public Fu11(), Fu12() As Double
   Public Fu21(), Fu22() As Double
   Public Fu1(), Fu2() As Double
   Public ma As Integer
   '各点大地坐标
   Public dadi1() As Double
   Public dadi2() As Double
'相对定向
Public Sub xddx()
   Dim i, j As Integer
   Dim isok As Boolean
   Dim shun As Integer       '控制总循环
   shun = 1
   '旋转矩阵元素
   Dim a1, a2, a3, b1, b2, b3, c1, c2, c3 As Double
   Dim fy1 As Double                  '求旋转矩阵的外方位元素
   Dim om1 As Double
   Dim kp1 As Double
   Dim fy2 As Double
   Dim om2 As Double
   Dim kp2 As Double
   Dim P() As Double                 '权阵
   Dim R1() As Double          '旋转矩阵
   Dim R2() As Double
   Dim xyzFu() As Double
   Dim N1 As Double                   '各点的投影系数
   Dim N2 As Double
   Dim by As Double                   '相对定向元素,及其改正数
   Dim bz As Double
   Dim dfy, dom, dkp, du, dv As Double
   Dim Q() As Double                  '各点上下视差
   Dim a() As Double                  '系数矩阵
   Dim a4, b4, c4, d4, e4 As Double        '系数矩阵元素
   Dim dx() As Double            '改正数
   
   '像对定向元素赋初值
   fy(1) = 0: fy2 = 0
   om(1) = 0: om2 = 0
   kp(1) = 0: kp2 = 0
   u(1) = 0
   V(1) = 0
   
   '模型点坐标重定义
   If num(1) > num(2) Then
      ma = num(1)
   Else
      ma = num(2)
   End If
   ReDim Preserve Xmo(1 To 2, 1 To ma)
   ReDim Preserve Ymo(1 To 2, 1 To ma)
   ReDim Preserve Zmo(1 To 2, 1 To ma)
   '辅助坐标重定义
   ReDim Fu11(1 To num(1), 1 To 3)
   ReDim Fu12(1 To num(1), 1 To 3)
   ReDim Fu21(1 To num(2), 1 To 3)
   ReDim Fu22(1 To num(2), 1 To 3)
   Do While (shun < 3)       '控制两个像对的求解
         If shun = 1 Then
            ReDim N11(1 To num(1))
            ReDim N12(1 To num(1))
         Else
            ReDim N21(1 To num(2))
            ReDim N22(1 To num(2))
         End If
         ReDim Q(1 To num(shun))
         '求外方位元素
         fy1 = fy2
         om1 = om2
         kp1 = kp2
         fy(shun) = 0
         om(shun) = 0
         kp(shun) = 0
         u(shun) = 0
         V(shun) = 0
         '改正数赋初值
         dfy = 0: dom = 0: dkp = 0: du = 0: dv = 0
         '求左片旋转矩阵
         Dim flag As Boolean
         flag = xunzhuan(fy1, om1, kp1, R1())
         
         '求权阵 P()
         ReDim P(1 To num(shun), 1 To num(shun))
         For i = 1 To num(shun)
             For j = 1 To num(shun)
                P(i, j) = 0
             Next j
             P(i, i) = 1
         Next i
         
         '迭代计算,求Q(),A()
         Do
            
            '相对定向元素的改正
            fy(shun) = fy(shun) + dfy
            om(shun) = om(shun) + dom
            kp(shun) = kp(shun) + dkp
            u(shun) = u(shun) + du
            V(shun) = V(shun) + dv
            
            '求右片的外方位元素
            fy2 = fy1 + fy(shun)
            om2 = om1 + om(shun)
            kp2 = kp1 + kp(shun)
            
            '求右片的旋转矩阵
            flag = xunzhuan(fy2, om2, kp2, R2())
            
            '求by,bz
            by = bx * u(shun): bz = bx * V(shun)
            bysave(shun) = by
            bzsave(shun) = bz
            '求同名象点的像空间辅助坐标
            Dim fuzhu1(1 To 3, 1 To 1) As Double
            Dim fuzhu2(1 To 3, 1 To 1) As Double
            ReDim Fu1(1 To num(shun), 1 To 3)
            ReDim Fu2(1 To num(shun), 1 To 3)
            ReDim Q(1 To num(shun), 1 To 1)
            ReDim a(1 To num(shun), 1 To 5)
            For i = 1 To num(shun)
                Select Case shun
                    Case 1
                       fuzhu1(1, 1) = x11(i): fuzhu1(2, 1) = y11(i)
                       fuzhu2(1, 1) = x12(i): fuzhu2(2, 1) = y12(i)
                    Case 2
                       fuzhu1(1, 1) = x21(i): fuzhu1(2, 1) = y21(i)
                       fuzhu2(1, 1) = x22(i): fuzhu2(2, 1) = y22(i)
                End Select
                fuzhu1(3, 1) = -f: fuzhu2(3, 1) = -f
                isok = multi(R1(), fuzhu1(), xyzFu())
                For j = 1 To 3
                    Fu1(i, j) = xyzFu(j, 1)
                Next j
                isok = multi(R2(), fuzhu2(), xyzFu())
                For j = 1 To 3
                    Fu2(i, j) = xyzFu(j, 1)
                Next j
                
                '保存像空间辅助坐标
                If shun = 1 Then
                   For j = 1 To 3
                      Fu11(i, j) = Fu1(i, j)
                   Next j
                   For j = 1 To 3
                      Fu12(i, j) = Fu2(i, j)
                   Next j
                 Else
                   For j = 1 To 3
                      Fu21(i, j) = Fu1(i, j)
                   Next j
                   For j = 1 To 3
                      Fu22(i, j) = Fu2(i, j)
                   Next j
                 End If
                '求N1,N2
                N1 = (bx * Fu2(i, 3) - bz * Fu2(i, 1)) / (Fu1(i, 1) * Fu2(i, 3) - Fu2(i, 1) * Fu1(i, 3))
                N2 = (bx * Fu1(i, 3) - bz * Fu1(i, 1)) / (Fu1(i, 1) * Fu2(i, 3) - Fu2(i, 1) * Fu1(i, 3))
                If shun = 1 Then
                   N11(i) = N1
                   N12(i) = N2
                Else
                   N21(i) = N1
                   N22(i) = N2
                End If
                '求Q , A
                Q(i, 1) = N1 * Fu1(i, 2) - N2 * Fu2(i, 2) - by
                a4 = bx
                b4 = -Fu2(i, 2) * bx / Fu2(i, 3)
                c4 = -Fu2(i, 1) * Fu2(i, 2) * N2 / Fu2(i, 3)
                d4 = -(Fu2(i, 3) + Fu2(i, 2) * Fu2(i, 2) / Fu2(i, 3)) * N2
                e4 = Fu2(i, 1) * N2
                a(i, 1) = a4
                a(i, 2) = b4
                a(i, 3) = c4
                a(i, 4) = d4
                a(i, 5) = e4
                '求模型点坐标
                Xmo(shun, i) = N1 * Fu1(i, 1)
                Ymo(shun, i) = (N1 * Fu1(i, 2) + N2 * Fu2(i, 2) + by) / 2
                Zmo(shun, i) = N1 * Fu1(i, 3)
         Next i
           '解法方程
            isok = solveFa1(a(), Q(), P(), dx())
            du = dx(1, 1): dv = dx(2, 1)
            dfy = dx(3, 1): dom = dx(4, 1): dkp = dx(5, 1)
         Loop Until (Abs(du) < e And Abs(dv) < e And Abs(dfy) < e And Abs(dom) < e And Abs(dkp) < e)
         shun = shun + 1
   Loop
End Sub
'模型连接
Public Sub mxlj()
  Dim same1(), same2() As Integer        '同名点序号
  Dim N1, N2 As Double           '像片投影系数
  Dim i, j, count As Integer
  '查找同名点
  count = 0
  For i = 1 To num(1)
     For j = 1 To num(2)
         If name1(i) = name2(j) Then
            count = count + 1
            ReDim Preserve same1(1 To count)
            ReDim Preserve same2(1 To count)
            same1(count) = i
            same2(count) = j
         End If
     Next j
  Next i
  
  Dim ki() As Double
  ReDim ki(1 To count)
  Dim k As Double
  k = 0
  '求归化系数
  For i = 1 To count
     ki(i) = (Zmo(1, same1(i)) - bzsave(1)) / Zmo(2, same2(i))
     k = k + ki(i)
  Next i
  k = k / count
  '统一比例尺,统一原点
  For i = 1 To num(2)
     Xmo(2, i) = Xmo(2, i) * k: Xmo(2, i) = Xmo(2, i) + bx
     Ymo(2, i) = Ymo(2, i) * k: Ymo(2, i) = Ymo(2, i) + bysave(1) * k
     Zmo(2, i) = Zmo(2, i) * k: Zmo(2, i) = Zmo(2, i) + bzsave(1) * k
  Next i
  For i = 1 To count
     Xmo(1, same1(i)) = (Xmo(1, same1(i)) + Xmo(2, same2(i))) / 2
     Xmo(2, same2(i)) = Xmo(1, same1(i))
     Ymo(1, same1(i)) = (Ymo(1, same1(i)) + Ymo(2, same2(i))) / 2
     Ymo(2, same2(i)) = Ymo(1, same1(i))
     Zmo(1, same1(i)) = (Zmo(1, same1(i)) + Zmo(2, same2(i))) / 2
     Zmo(2, same2(i)) = Zmo(1, same1(i))
  Next i
End Sub
'绝对定向
Public Sub jddx()
  Dim i, j As Integer
  Dim a, b, nenm As Double
  Dim kxc(), kyc(), kzc() As Double          '控制点参考坐标
  Dim dx, dy, dxt, dyt As Double             '用于求a,b,λ
  Dim kongmox() As Double                    '控制点的模型坐标
  Dim kongmoy() As Double
  Dim kongmoz() As Double
  '求控制点的模型坐标
  ReDim kongmox(1 To num(3))
  ReDim kongmoy(1 To num(3))
  ReDim kongmoz(1 To num(3))
  For i = 1 To num(1)
    For j = 1 To num(3)
       If name3(j) = name1(i) Then
          kongmox(j) = Xmo(1, i)
          kongmoy(j) = Ymo(1, i)
          kongmoz(j) = Zmo(1, i)
       End If
    Next j
  Next i
  For i = 1 To num(2)
    For j = 1 To num(3)
       If name3(j) = name2(i) Then
          kongmox(j) = Xmo(2, i)
          kongmoy(j) = Ymo(2, i)
          kongmoz(j) = Zmo(2, i)
       End If
    Next j
  Next i
  
  '求a,b,λ
  dx = kongmox(3) - kongmox(1)
  dy = kongmoy(3) - kongmoy(1)
  dxt = xkong(3) - xkong(1)
  dyt = ykong(3) - ykong(1)
  a = (dx * dxt - dy * dyt) / (dxt * dxt + dyt * dyt)
  b = (dx * dyt + dy * dxt) / (dxt * dxt + dyt * dyt)
  nenm = Sqr((dx * dx + dy * dy) / (dxt * dxt + dyt * dyt))
  
  '求控制点参考坐标系坐标
  ReDim kxc(1 To num(3)), kyc(1 To num(3)), kzc(1 To num(3))
  Dim zhong1() As Double
  Dim zhong2() As Double
  Dim myflag As Boolean
  Dim juzhen() As Double
  ReDim juzhen(1 To 3, 1 To 3)
  ReDim zhong1(1 To 3, 1 To 1)
  juzhen(1, 1) = a: juzhen(1, 2) = b: juzhen(1, 3) = 0
  juzhen(2, 1) = b: juzhen(2, 2) = -a: juzhen(2, 3) = 0
  juzhen(3, 1) = 0: juzhen(3, 2) = 0: juzhen(3, 3) = nenm
  Dim mmm, nnn As Double
  mmm = xkong(1)
  nnn = ykong(1)
  For i = 1 To num(3)
    
     xkong(i) = xkong(i) - mmm
     ykong(i) = ykong(i) - nnn
     zkong(i) = zkong(i)
     zhong1(1, 1) = xkong(i)
     zhong1(2, 1) = ykong(i)
     zhong1(3, 1) = zkong(i)

     myflag = multi(juzhen(), zhong1(), zhong2())
     kxc(i) = zhong2(1, 1)
     kyc(i) = zhong2(2, 1)
     kzc(i) = zhong2(3, 1)
  Next i
  
  Dim jfy, jom, jkp, jnenm, jXs, jYs, jZs As Double              '绝对定向元素
  Dim jdfy, jdom, jdkp, jdnenm, jdXs, jdYs, jdZs As Double       '绝对定向元素的改正值
  jdfy = 0: jdom = 0: jdkp = 0: jdnenm = 0: jdXs = 0: jdYs = 0: jdZs = 0
  jfy = 0: jom = 0: jkp = 0: jnenm = 1: jXs = 0: jYs = 0: jZs = 0
  Dim R() As Double           '旋转矩阵
  Dim jA() As Double
  Dim jL() As Double
  ReDim jA(1 To 3 * num(3), 1 To 7)
  ReDim jL(1 To 3 * num(3), 1 To 1)
  '系数矩阵赋值
  For i = 1 To num(3)
     jA((i - 1) * 3 + 1, 5) = 1
     jA((i - 1) * 3 + 1, 6) = 0
     jA((i - 1) * 3 + 1, 7) = 0
     
     jA((i - 1) * 3 + 2, 5) = 0
     jA((i - 1) * 3 + 2, 6) = 1
     jA((i - 1) * 3 + 2, 7) = 0
     
     jA((i - 1) * 3 + 3, 5) = 0
     jA((i - 1) * 3 + 3, 6) = 0
     jA((i - 1) * 3 + 3, 7) = 1
     
     jA((i - 1) * 3 + 1, 1) = 0
     jA((i - 1) * 3 + 1, 2) = -kongmoz(i)
     jA((i - 1) * 3 + 1, 3) = -kongmoy(i)
     jA((i - 1) * 3 + 1, 4) = kongmox(i)
     
     jA((i - 1) * 3 + 2, 1) = -kongmoz(i)
     jA((i - 1) * 3 + 2, 2) = 0
     jA((i - 1) * 3 + 2, 3) = kongmox(i)
     jA((i - 1) * 3 + 2, 4) = kongmoy(i)
     
     jA((i - 1) * 3 + 3, 1) = kongmoy(i)
     jA((i - 1) * 3 + 3, 2) = kongmox(i)
     jA((i - 1) * 3 + 3, 3) = 0
     jA((i - 1) * 3 + 3, 4) = kongmoz(i)
  Next i
  Do
     '绝对定向元素的改正
     jfy = jfy + jdfy
     jom = jom + jdom
     jkp = jkp + jdkp
     jnenm = jnenm + jdnenm
     jXs = jXs + jdXs
     jYs = jYs + jdYs
     jZs = jZs + jdZs
     '求旋转矩阵
     ReDim R(1 To 3, 1 To 3)

     myflag = xunzhuan(jfy, jom, jkp, R())
     '求L
     ReDim zhong1(1 To 3, 1 To 1)
     Dim jian() As Double
     For i = 1 To num(3)
         zhong1(1, 1) = kongmox(i): zhong1(2, 1) = kongmoy(i): zhong1(3, 1) = kongmoz(i)
         myflag = multi(R(), zhong1(), jian())
         jL((i - 1) * 3 + 1, 1) = kxc(i) - jnenm * jian(1, 1) - jXs
         jL((i - 1) * 3 + 2, 1) = kyc(i) - jnenm * jian(2, 1) - jYs
         jL((i - 1) * 3 + 3, 1) = kzc(i) - jnenm * jian(3, 1) - jZs
     Next i
     '解算绝对定向元素
     Dim AT() As Double
     Dim sleft() As Double
     Dim sright() As Double
     Dim zhong3() As Double
     Dim niz() As Double
     myflag = zhuanzhi(jA(), AT())
     myflag = multi(AT(), jA(), sleft())
     myflag = multi(AT(), jL(), sright())
     myflag = invers(sleft(), niz())
     myflag = multi(niz(), sright(), zhong3())
     '绝对定向元素改正
     jdom = zhong3(1, 1)
     jdfy = zhong3(2, 1)
     jdkp = zhong3(3, 1)
     jdnenm = zhong3(4, 1)
     jdXs = zhong3(5, 1)
     jdYs = zhong3(6, 1)
     jdZs = zhong3(7, 1)
  Loop Until (Abs(jdom) < e And Abs(jdfy) < e And Abs(jdkp) < e And Abs(jdXs) < e And Abs(jdYs) < e And Abs(jdZs) < e)
  
  Dim dao1() As Double
  Dim dao2() As Double
  Dim dao3(1 To 3, 1 To 1) As Double
  Dim dao4() As Double
  ReDim dadi1(1 To num(1), 1 To 3)
  ReDim dadi2(1 To num(2), 1 To 3)

  myflag = xunzhuan(jfy, jom, jkp, R())
  Dim jdd As Integer