Attribute VB_Name = "Module1"
Rem Global (external) VARS.
Rem Program does not use any DLL-calls.

Rem Maximum number of allowed image observations (See Type declaration for Image_observation below)
Public Const MAX_OBS = 15000
Rem Maximum number of allowed images  in the block (Type cameraparameters)
Public Const MAX_IMA = 1060
Rem Maximum number of allowed TIE and Groung Control Points (GCP) (Type/struct Ground observation)
Public Const MAX_GND = 4000
Rem Maximum number of iterations when computing space intersection
Public Const MAX_ITERATIONS = 100
Rem Maximum number of cameras in space intersection
Public Const IMA_NUM = 150
Rem The 3D-distance of the correction vector below which iteration (ray.intersection) is stopped
Rem and the 3D-coordinates are no longer improved
Public Const ITE_STOP = 0.001


Type Image_observation
  Point As Long  ' point number (id)
  image As Long  ' image number
  weight As Single
  x As Double       ' x-coordinate
  y As Double       ' y-coordinate
End Type

Type Ground_observation
  Point As Long  ' point number (id)
  Type As Long   ' 0 = Tie point, 1 = GCP, 2 = Elev.Point
  wX As Single   ' weight, X
  WY As Single
  WZ As Single
  x As Double       ' x-coordinate
  y As Double       ' y-coordinate
  z As Double       ' y-coordinate
  x_ini As Double
  y_ini As Double
  z_ini As Double
End Type

Type cameraparameters
  c As Double      ' camera constant
  image As Long    ' image number
  omega As Double  ' pan
  phi As Double    ' tilt
  kappa As Double  ' rotation
  x0 As Double     ' proj. center x
  y0 As Double     '   "     "    y
  z0 As Double     '   "     "    z
  X0_GPS As Double
  Y0_GPS As Double
  Z0_GPS As Double
  omega_IMU As Double
  phi_IMU As Double
  kappa_IMU As Double
  Camname As String * 10 ' camera name
  Type As Byte
  wx0 As Single
  wy0 As Single
  wz0 As Single
  womega As Single
  wphi As Single
  wkappa As Single
End Type

Type ColumnIDCam
 Cam_ID As Long
 omega_ID As Long
 phi_ID As Long
 kappa_ID As Long
 X0_ID As Long
 Y0_ID As Long
 Z0_ID As Long
End Type

Type ColIDPoint
 Pt_ID As Long
 X_ID As Long
 Y_ID As Long
 Z_ID As Long
End Type
 
Type Z_pairs
 ID_1 As Long
 ID_2 As Long
End Type

Type XY_pairs
 ID_1 As Long
 ID_2 As Long
End Type

Rem Declarations for observations counts
Public Num_Of_Images As Long
Public Num_Of_GCPS As Long
Public Num_Of_Tie_Points As Long
Public Num_Of_Elev_Points As Long
Public Num_Of_Image_Obs As Long
Public Num_Of_GPSIMU As Long
Public Num_Of_Z_Pairs As Long
Public Num_Of_XY_Pairs As Long

Rem Declarations for parameter Arrays
Public Ima_obs(1 To MAX_OBS) As Image_observation  ' array for image observations
Public Gnd_obs(1 To MAX_GND) As Ground_observation ' array for XYZ observations / unknowns
Public Cam_obs(1 To MAX_IMA) As cameraparameters   ' array for unknown camera parameters
Public ColIDCam(1 To MAX_IMA) As ColumnIDCam       ' array for holding indeces to cols in the design matrix
Public ColIDPt(1 To MAX_GND) As ColIDPoint      ' array for holding indeces to cols in the design matrix
Public Z_pairs(1 To MAX_GND) As Z_pairs            ' array with Point IDs
Public XY_pairs(1 To MAX_GND) As XY_pairs          ' array with Point IDs

Rem Coefficient Matrix A. A'A = Normal matrix of adjustment process.

Public A() As Single
Rem INV(A'A)*A'l = x, x = CORR_VECT (see l-vector below), vector for corrections of the unknowns
Public CORR_VECT(1 To (MAX_IMA * 6 + MAX_GND * 3)) As Double
Rem Approxomated - measured (obs), Normal vector, l

Public l() As Double
Rem Rotation matrices for images, first element defines the image number, followed
Rem by the 3 x 3 rotation matrices. See Calculate_Rotation_Matrices()-sub for definitions.
Public R(1 To MAX_IMA, 1 To 3, 1 To 3)

Declare Function MYFUNC_MATLABCALL Lib "c:\data\pascaldll.dll" (ByVal v1 As Double) As Double













Public Sub Calculate_Rotation_Matrices()

Dim omega, phi, kappa As Double
Dim i, j As Long

For i = 1 To Num_Of_Images
 j = Cam_obs(i).image
 omega = Cam_obs(i).omega: phi = Cam_obs(i).phi: kappa = Cam_obs(i).kappa
 R(j, 1, 1) = Cos(phi) * Cos(kappa): R(j, 1, 2) = -Cos(phi) * Sin(kappa): R(j, 1, 3) = Sin(phi)
 R(j, 2, 1) = Cos(omega) * Sin(kappa) + Sin(omega) * Sin(phi) * Cos(kappa): R(j, 2, 2) = Cos(omega) * Cos(kappa) - Sin(omega) * Sin(phi) * Sin(kappa): R(j, 2, 3) = -Sin(omega) * Cos(phi)
 R(j, 3, 1) = Sin(omega) * Sin(kappa) - Cos(omega) * Sin(phi) * Cos(kappa): R(j, 3, 2) = Sin(omega) * Cos(kappa) + Cos(omega) * Sin(phi) * Sin(kappa): R(j, 3, 3) = Cos(omega) * Cos(phi)

Next i
End Sub

Public Sub Calculate_Differential_quotiens(ByVal obs As Integer, dx_domega_K, dy_domega_K, dx_dphi_K, dy_dphi_K, dx_dkappa_K, dy_dkappa_K, dx_dU_K, dy_dU_K, dx_dU0_K, dy_dU0_K, dx_dV_K, dy_dV_K, dx_dV0_K, dy_dV0_K, dx_dW_K, dy_dW_K, dx_dW0_K, dy_dW0_K, dx, dy)


Rem This subroutine calculates the differential quotiens needed for the matrix A & errors dx, dy (vector l)
Rem The quotients are:
Rem dx_domega_K,  dy_domega_K,  dx_dphi_K,  dy_dphi_K, dx_dkappa_K,  dy_dkappa_K
Rem dx_dU_K,      dy_dU_K,      dx_dU0_K,   dy_dU0_K,  dx_dV_K,      dy_dV_K,
Rem dx_dV0_K,     dy_dV0_K,     dx_dW_K,    dy_dW_K,   dx_dW0_K ,    dy_dW0_K


Rem VAR declarations (NOTE U=X, V=Y, W=Z as in Krauss) (Fotogrammetric tradition?)
Dim omega As Double, phi As Double, kappa As Double
Dim x0 As Double, y0 As Double, z0 As Double
Dim x As Double, y As Double, z As Double, c As Double
Dim x_k As Double, y_k As Double, Zx As Double, Zy As Double, n  As Double
Dim U As Double, V As Double, W As Double, U0 As Double, V0 As Double, W0 As Double
Dim j As Integer

Rem Passed argument obs 'points' to array Ima_obs() that contains elements
Rem (point, image, x,y)

j = obs

Rem Assign camera coordinates
x = Ima_obs(j).x:  y = Ima_obs(j).y:   z = 0#

Rem Assign inner orientation parameters (camera constant = z0) (all coords have been translated
Rem to center of symmetry => x0, y0 are zero)

x0 = 0#:           y0 = 0#:
 
Rem Search for the rigth point in gnd_obs()
 
 For i = 1 To (Num_Of_GCPS + Num_Of_Tie_Points + Num_Of_Elev_Points)
   If Ima_obs(j).Point = Gnd_obs(i).Point Then
     U = Gnd_obs(i).x:  V = Gnd_obs(i).y:   W = Gnd_obs(i).z
     GoTo Point_Found
   End If
 Next i
 
 Rem We should find the point, if not notify & exit
 MsgBox ("The X,Y,Z data was not found for a point in Calculate_Differential quotiens!")
 Exit Sub

Point_Found:

Rem Search the right element in cam_obs() array, to get the camera parameters

 For i = 1 To Num_Of_Images
  If Ima_obs(j).image = Cam_obs(i).image Then
   U0 = Cam_obs(i).x0: V0 = Cam_obs(i).y0: W0 = Cam_obs(i).z0:   z0 = Cam_obs(i).c
   omega = Cam_obs(i).omega: phi = Cam_obs(i).phi: kappa = Cam_obs(i).kappa
   GoTo Image_Found
  End If
 Next i
 Rem We should find the camera, if not notify & exit
 MsgBox ("The X0,Y0,Z0,om,ph,ka -data was not found for an image in Calculate_Differential quotiens!")
 Exit Sub
 
Image_Found:

j = Cam_obs(i).image

Rem To shorten the equtions below use the following common factors

 Zx = R(j, 1, 1) * (U - U0) + R(j, 2, 1) * (V - V0) + R(j, 3, 1) * (W - W0)
 Zy = R(j, 1, 2) * (U - U0) + R(j, 2, 2) * (V - V0) + R(j, 3, 2) * (W - W0)
 n = R(j, 1, 3) * (U - U0) + R(j, 2, 3) * (V - V0) + R(j, 3, 3) * (W - W0)
 c = (z - z0)

Rem Compute the differential qoutients


 dx_domega_K = (-c / n * (((V - V0) * R(j, 3, 3) - (W - W0) * R(j, 2, 3)) * Zx / n - (V - V0) * R(j, 3, 1) + (W - W0) * R(j, 2, 1))) * 10 ^ 6
 dy_domega_K = (-c / n * (((V - V0) * R(j, 3, 3) - (W - W0) * R(j, 2, 3)) * Zy / n - (V - V0) * R(j, 3, 2) + (W - W0) * R(j, 2, 2))) * 10 ^ 6
 dx_dphi_K = (c / n * ((Zx * Cos(kappa) - Zy * Sin(kappa)) * Zx / n + n * Cos(kappa))) * 10 ^ 6
 dy_dphi_K = (c / n * ((Zx * Cos(kappa) - Zy * Sin(kappa)) * Zy / n - n * Sin(kappa))) * 10 ^ 6
 dx_dkappa_K = (-c / n * Zy) * 10 ^ 6
 dy_dkappa_K = (c / n * Zx) * 10 ^ 6
 dx_dU_K = (-c / (n ^ 2)) * (n * R(j, 1, 1) - Zx * R(j, 1, 3)) * 10 ^ 6
 dy_dU_K = (-c / (n ^ 2)) * (n * R(j, 1, 2) - Zy * R(j, 1, 3)) * 10 ^ 6
 dx_dU0_K = (-c / (n ^ 2)) * (R(j, 1, 3) * Zx - R(j, 1, 1) * n) * 10 ^ 6
 dy_dU0_K = (-c / (n ^ 2)) * (-n * R(j, 1, 2) + Zy * R(j, 1, 3)) * 10 ^ 6
 dx_dV_K = (-c / (n ^ 2)) * (n * R(j, 2, 1) - Zx * R(j, 2, 3)) * 10 ^ 6
 dy_dV_K = (-c / (n ^ 2)) * (n * R(j, 2, 2) - Zy * R(j, 2, 3)) * 10 ^ 6
 dx_dV0_K = (-c / (n ^ 2)) * (Zx * R(j, 2, 3) - n * R(j, 2, 1)) * 10 ^ 6
 dy_dV0_K = (-c / (n ^ 2)) * (R(j, 2, 3) * Zy - R(j, 2, 2) * n) * 10 ^ 6
 dx_dW_K = (-c / (n ^ 2)) * (n * R(j, 3, 1) - Zx * R(j, 3, 3)) * 10 ^ 6
 dy_dW_K = (-c / (n ^ 2)) * (n * R(j, 3, 2) - Zy * R(j, 3, 3)) * 10 ^ 6
 dx_dW0_K = (-c / (n ^ 2)) * (Zx * R(j, 3, 3) - n * R(j, 3, 1)) * 10 ^ 6
 dy_dW0_K = (-c / (n ^ 2)) * (Zy * R(j, 3, 3) - n * R(j, 3, 2)) * 10 ^ 6

Rem With the parameters 'at hand', Calculate the image coordinates given the ground point

 x_k = x0 + (z - z0) * (R(j, 1, 1) * (U - U0) + R(j, 2, 1) * (V - V0) + R(j, 3, 1) * (W - W0)) / (R(j, 1, 3) * (U - U0) + R(j, 2, 3) * (V - V0) + R(j, 3, 3) * (W - W0))
 y_k = y0 + (z - z0) * (R(j, 1, 2) * (U - U0) + R(j, 2, 2) * (V - V0) + R(j, 3, 2) * (W - W0)) / (R(j, 1, 3) * (U - U0) + R(j, 2, 3) * (V - V0) + R(j, 3, 3) * (W - W0))
 
Rem calculate error (residual) (x,y have the observed coords)

 dx = x_k - x
 dy = y_k - y


End Sub

Public Sub Add_Correction_vector()

Dim i As Integer, j As Integer, k As Integer

Rem This subroutine adds the elements of the solution of the adjustment process (correction vector,
Rem containing adjustments to the unknown camera parameters & object coordinates (X,Y,Z) of the tie
Rem Points. NOTE there are only FULL control points in the sence that all three coordinates must be
Rem known) to the initial values (current values of) (updates them).

Rem First add the camera parameters, six per camera

i = 0

For j = 1 To 6 * Num_Of_Images Step 6
  i = i + 1
  Cam_obs(i).omega = Cam_obs(i).omega + CORR_VECT(j)
  Cam_obs(i).phi = Cam_obs(i).phi + CORR_VECT(j + 1)
  Cam_obs(i).kappa = Cam_obs(i).kappa + CORR_VECT(j + 2)
  Cam_obs(i).x0 = Cam_obs(i).x0 + CORR_VECT(j + 3)
  Cam_obs(i).y0 = Cam_obs(i).y0 + CORR_VECT(j + 4)
  Cam_obs(i).z0 = Cam_obs(i).z0 + CORR_VECT(j + 5)
  Rem Form1.Label1(Cam_obs(i).image).Caption = Format$(Cam_obs(i).image, "0") & " " & Format$(Cam_obs(i).omega, "#.00000000") & " " & Format$(Cam_obs(i).phi, "#.00000000") & " " & Format$(Cam_obs(i).kappa, "#.00000000") & " " & Format$(Cam_obs(i).x0, "#.00000000") & " " & Format$(Cam_obs(i).y0, "#.00000000") & " " & Format$(Cam_obs(i).z0, "#.00000000")
Next j

Rem Find the first tie point in gnd_obs(). -struct with gnd_obs().Type set to 0
Rem Should be Num_Of_GCPS + 1

Rem Loop thru all
For j = 1 To (Num_Of_Tie_Points)
 If Gnd_obs(j).Type = 0 Then GoTo First_Tie_Point_Found   ' First was found
Next j

Rem If we fail to find it, it is a fatal error.
MsgBox ("In Routine add-correction vector, failed to find the first Tie_Point!")
'Exit Sub

First_Tie_Point_Found:

Rem Check that the index is 1, 4, 7, 10,... (because each tie point has three unknown
Rem coordinates ) and partial control points (knwon XY or Z) are not allowed.
Rem

If j <> 1 Then
 MsgBox ("In add_Correction vector, unexpected index for the last control point!")
End If
Rem Let k loop thru tie points, i steps in three (X,Y,Z)
k = j - 1
For i = 6 * Num_Of_Images + 1 To (6 * Num_Of_Images + 3 * Num_Of_Tie_Points) Step 3
 k = k + 1
 Gnd_obs(k).x = Gnd_obs(k).x + CORR_VECT(i)
 Gnd_obs(k).y = Gnd_obs(k).y + CORR_VECT(i + 1)
 Gnd_obs(k).z = Gnd_obs(k).z + CORR_VECT(i + 2)
Next i

MsgBox ("The unknowns have been updated/omitted succesfully!")

End Sub

Public Sub Print_Normal_Matrix_and_Vector()

Dim capu As String
Dim i As Long, j As Long

Rem Matrix A and vector l are ready to be outputted for future Matlab input and inversion + solution of CORR_VECT.
Rem Solution, correction vector, x = inv(A'A)*A'l

Rem The dimensions of A are (2 * Num_of_image_obs) rows * (6* Num_Of_Images + 3 * Num_Of_Tie_Points) columns
Rem The dimensions of l are (2* Num_of_Image_obs) rows * 1 column

Open "C:\data\l.txt" For Binary As 1
Close (11)
Open "C:\data\l_txt.txt" For Output As 11
k = -7
  For j = 1 To 2 * Num_Of_Image_Obs + 6 * Num_Of_GPSIMU + 3 * Num_Of_Tie_Points + Num_Of_Z_Pairs + 2 * Num_Of_XY_Pairs   '+ 2
  k = k + 8
  Put #1, k, l(j)
  If j <= 2 * Num_Of_Image_Obs Then
    Print #11, Format$(l(j) * 10 ^ 6, "#.0000000000000000000")
  Else
    Print #11, Format$(l(j) * 10 ^ 6, "#.0000000000000000000")
  End If
  If j / 2 <= Num_Of_Image_Obs Then
'  Form1.List1.AddItem "IM error Point: " & Format$(Ima_obs((j + j Mod 2) / 2).Point, "#") & " " & Format$(Ima_obs((j + j Mod 2) / 2).image, "#") & " " & Format$(l(j), "#.000000")
  End If
  Next j
Close (1)
Close (11)

Rem MsgBox ("Dimensions of l are " & CStr(2 * Num_Of_Image_Obs) & " Rows and 1 column")
End Sub

Public Sub Calc_dX_dY_dZ_dx_dy(ByVal iobs As Integer, ByVal gobs As Integer, ByVal cam As Integer, x_ini, y_ini, z_ini, dx_dU_K, dy_dU_K, dx_dV_K, dy_dV_K, dx_dW_K, dy_dW_K, dx, dy)

Rem This subroutine calculates the differential quotiens needed in the calculation of space intersection
Rem (3D-reconstruction).

Rem Passed arguments:
Rem iobs => ima_obs(iobs), index to image observation (rows (2) in Matrix A)
Rem gops => gnd_obs(gops), index to ground observation
Rem cam  => cam_obs(x).image  (omega, phi, kappa, xo, y0, zo), image number (not index)
Rem X_ini, Y_ini, Z_ini (initial / iteratively improved) XYZ-coordinates (of Tie points)

Rem Returned arguments.  ! Notation U=X, V=Y, W=Z, (x,y) are camera coordinates!
Rem dx_dU_K, dy_dU_K, dx_dV_K, dy_dV_K, dx_dW_K, dy_dW_K, dx, dy

Rem NOTE notation from textbook of Krauss) U = X, V = Y, W = Z

Dim omega As Double, phi As Double, kappa As Double
Dim U As Double, V As Double, W As Double, U0 As Double, V0 As Double, W0 As Double
Dim x As Double, y As Double, z As Double, z0 As Double, x0 As Double, y0 As Double
Dim x_k As Double, y_k As Double
Dim Zx As Double, Zy As Double, n As Double, c As Double
Dim i As Integer, c_index As Integer

Rem The SUB is passed the cam -variable (image number)


For i = 1 To Num_Of_Images
 If Cam_obs(i).image = cam Then
  c_index = i
 End If
Next i

Rem c_index now 'points' to the correct index in cam_obs() -array.

Rem Assign passed X,Y,Z to U,V,W (point coordinates to be improved by iteration)

U = x_ini:                  V = y_ini:              W = z_ini


Rem Update camera-angles

omega = Cam_obs(c_index).omega: phi = Cam_obs(c_index).phi: kappa = Cam_obs(c_index).kappa

Rem Get the image observations for this point (U,V,W)

x = Ima_obs(iobs).x: y = Ima_obs(iobs).y

Rem Assign values to the principal point of symmetry (everything has been PPS-translated)
z = 0#
x0 = 0#: y0 = 0#

Rem NOTE Focal length should be imported elsewhere if aerial triangulation is done
Rem with images taken with different cameras (or same camera different calibration)

U0 = Cam_obs(c_index).x0: V0 = Cam_obs(c_index).y0: W0 = Cam_obs(c_index).z0
z0 = Cam_obs(c_index).c
' MsgBox ("U0, Vo, W0: " & CStr(U0) & " " & CStr(V0) & " " & CStr(W0))
' MsgBox ("R (cam, #, 3: " & CStr(R(cam, 1, 3)) & " " & CStr(R(cam, 2, 3)) & " " & CStr(R(cam, 3, 3)))

Zx = R(cam, 1, 1) * (U - U0) + R(cam, 2, 1) * (V - V0) + R(cam, 3, 1) * (W - W0)
Zy = R(cam, 1, 2) * (U - U0) + R(cam, 2, 2) * (V - V0) + R(cam, 3, 2) * (W - W0)
n = R(cam, 1, 3) * (U - U0) + R(cam, 2, 3) * (V - V0) + R(cam, 3, 3) * (W - W0)
c = (z - z0)
 
dx_dU_K = (-c / (n ^ 2)) * (n * R(cam, 1, 1) - Zx * R(c_index, 1, 3))
dy_dU_K = (-c / (n ^ 2)) * (n * R(cam, 1, 2) - Zy * R(c_index, 1, 3))
dx_dV_K = (-c / (n ^ 2)) * (n * R(cam, 2, 1) - Zx * R(cam, 2, 3))
dy_dV_K = (-c / (n ^ 2)) * (n * R(cam, 2, 2) - Zy * R(cam, 2, 3))
dx_dW_K = (-c / (n ^ 2)) * (n * R(cam, 3, 1) - Zx * R(cam, 3, 3))
dy_dW_K = (-c / (n ^ 2)) * (n * R(cam, 3, 2) - Zy * R(cam, 3, 3))


' Calculate the image coordinates from the ground point
x_k = x0 + (z - z0) * (R(cam, 1, 1) * (U - U0) + R(cam, 2, 1) * (V - V0) + R(cam, 3, 1) * (W - W0)) / (R(cam, 1, 3) * (U - U0) + R(cam, 2, 3) * (V - V0) + R(cam, 3, 3) * (W - W0))
y_k = y0 + (z - z0) * (R(cam, 1, 2) * (U - U0) + R(cam, 2, 2) * (V - V0) + R(cam, 3, 2) * (W - W0)) / (R(cam, 1, 3) * (U - U0) + R(cam, 2, 3) * (V - V0) + R(cam, 3, 3) * (W - W0))
' calculate error (residual)
dx = x_k - x
dy = y_k - y


End Sub
Function det_cal2(E() As Double) As Double
Rem This function calculates the determinant of a 2x2 passed matrix (E)
Dim term1, term2 As Double
term1 = E(1, 1) * E(2, 2)
term2 = E(2, 1) * E(1, 2)
det_cal2 = term1 - term2
End Function

Function det_cal3(E() As Double) As Double
Rem This function calculates the determinant of a 3x3 passed matrix (E)
Dim term1, term2, term3, term4, term5, term6 As Double
term1 = E(1, 1) * E(2, 2) * E(3, 3)
term2 = E(2, 1) * E(3, 2) * E(1, 3)
term3 = E(3, 1) * E(1, 2) * E(2, 3)
term4 = E(1, 1) * E(3, 2) * E(2, 3)
term5 = E(2, 1) * E(1, 2) * E(3, 3)
term6 = E(3, 1) * E(2, 2) * E(1, 3)
det_cal3 = term1 + term2 + term3 - term4 - term5 - term6
End Function