Attribute VB_Name = "ADS40_Module"
Rem Nov 26, 2008. Ilkka Korpela
Rem This module holds declarations and routines that are needed
Rem for the ads40 -imagery, which is a line sensor by Leica

Rem This struct holds the interior orientation of a sensor line (1 out of 11)
Type ADS40LineStruct
 CalibrationFile As Integer
 CalibrationSource As String * 30
 CameraName As String * 10
 SensorLine As String * 12
 CalibrationDate As String * 20
 FocalLength As Double
 NumOfPixels As Long
 PixelSize As Double
 RadiometricGain As Double
 IrradianceGain As Double
 LeftSideGain As Double
 PAV_Z_Offset As Double
End Type

Rem When reading the ODFs, this is the 40-byte READ-structure
Rem Since the STDEVs are stored as Unsigned SHORT -values, they are read by a c-function
Type ODFReadstruct
  GPStime As Long
  X As Long
  Y As Long
  z As Long
  omega As Long
  phi As Long
  kappa As Long
  StdDevX As Integer
  StdDevY As Integer
  StdDevZ As Integer
  StdDevOmega As Integer
  StdDevPhi As Integer
  StdDevKappa As Integer
End Type
Rem For storing the ODFs, need another struct.
Rem Each array (image) is of different length! Let's make then contain max 90000
Public Const MAXADS40LINES = 110000
Type ODFStruct
  GPStime As Double
  X As Double
  Y As Double
  z As Double
  omega As Double
  phi As Double
  kappa As Double
  a00 As Double
  a01 As Double
  a02 As Double
  a10 As Double
  a11 As Double
  a12 As Double
  a20 As Double
  a21 As Double
  a22 As Double
  StdDevX As Single
  StdDevY As Single
  StdDevZ As Single
  StdDevOmega As Single
  StdDevPhi As Single
  StdDevKappa As Single
End Type

Rem Data in the support file
Type ADS40Support
  IMAGE_ID As String * 30
  IMAGE_FILE_NAME As String * 100
  LINES As Long
  SAMPLES As Long
  RECTIFICATION_TERMS(1 To 6) As Double
  GRND_BIAS(1 To 3) As Double
  BIAS As Double
  GAIN As Double
  GROUND_ZERO As point3D
  LOAD_PT As point3D
  COORD_SYSTEM As Byte
  UNITS As Byte
  USE_FAST_MATH As Byte
  MIN_AND_MAX_ELEVATION As Point2D
  IMAGE_MOTION As Byte
  INITIALIZED  As Byte
  Status As Byte
  PHOTO_DATE As String * 30
  QUALITY As Byte
  SENSOR_TYPE As String * 10
  IMAGE_LEVEL As Byte
  MEAN_TERRAIN_HEIGHT As Double
  ANCHOR_LATITUDE As Double
  ANCHOR_LONGITUDE As Double
  LINE_STAGGERED As Boolean
  VIEW_OF_LINE As String * 12
  RADIOMETRIC_PROPERTY As String * 20
  IMAGE_DATE As String * 30
  NUMBER_SCAN_LINES As Long
  SENSOR_ROTATION As Boolean
  CORRECTION As String * 20
  LEVEL_0_GSD As Double
  ORIGINAL_ORIENTATION As String * 160
  ADJUSTED_ORIENTATION As String * 160
  ORIGINAL_CALIBRATION As String * 160
  ADJUSTED_CALIBRATION As String * 160
  LEVEL_0_NAME As String * 80
  RECT_SCALE As Double
  RECT_ROTATION As Double
  RECT_XOFFSET As Double
  RECT_YOFFSET As Double
  RECT_HEIGHT As Double
  RECT_USE_CALIBRATION As Boolean
  LOCAL_ORIGIN As point3D
  BASIS(1 To 3, 1 To 3) As Double
End Type

Type BlockData
 L0 As Long
 L1 As Long
 FileName As String
End Type

Type ADSHDR ' data in the *.ads file
 HEADER As Byte
 BANDS As Byte
 DEPTH As Byte
 BITS As Byte
 LINES As Long
 SAMPLES As Long
 TILE_X As Long
 TILE_Y As Long
 HARDWARE_COMPRESSED As Byte
 LINES_PER_BLOCK As Long
 SAMPLES_PER_BLOCK As Long
 BLOCK_DATA(0 To MAXIMA - 1, 1 To 3) As BlockData
 NUMBER_OF_SUBIMAGES As Long
End Type

Type ads40_image_point_struct
 Sample As Double
 Line As Double
End Type


Type ADS_obs
 lp As ads40_image_point_struct  ' line/sample coordinates
 fp As ads40_image_point_struct  ' position in the focal plane
 xy As Point2D                   ' same in xy-notation
End Type

Public ADSOBS(0 To MAXIMA - 1) As ADS_obs

Public PrevPoint As point3D
Public imgPt(0 To MAXIMA - 1) As ads40_image_point_struct

Public Const DEF_WINDOW_WIDTH = 20
Public Const DEF_WINDOW_HEIGHT = 400

Rem PUBLIC DATA TYPES
Public ADSHDR(0 To MAXIMA - 1) As ADSHDR
Public ADSSUP(0 To MAXIMA - 1) As ADS40Support
Public ODFs(0 To MAXADS40LINES - 1, 0 To MAXIMA - 1) As ODFStruct
Public ADSPixels(0 To 12000 - 1, 0 To MAXIMA - 1) As Point2D
Public ADSCam(0 To MAXIMA - 1) As ADS40LineStruct
Declare Function ADS40_READODFFILE Lib "c:\data\pascaldll.dll" (ByRef N_ODF As Long, ByRef FileToRead As Byte, ByRef STDARRAY As Long) As Long
Declare Function WGS84XYZ_TO_UTM Lib "c:\data\kuvamittgeotrans.dll" (ByVal X As Double, ByVal Y As Double, ByVal z As Double, ByRef Lat As Double, ByRef Lon As Double, ByRef Height As Double, ByRef UTM_E As Double, ByRef UTM_N As Double, ByRef Zone As Long) As Long
Declare Function UTM_TO_WGS84XYZ Lib "c:\data\kuvamittgeotrans.dll" (ByRef X As Double, ByRef Y As Double, ByRef z As Double, ByRef Lat As Double, ByRef Lon As Double, ByVal Height As Double, ByVal UTM_E As Double, ByVal UTM_N As Double, ByVal Zone As Long) As Long

Public Function point_line_distance(A As ads40_image_point_struct, p1 As ads40_image_point_struct, p2 As ads40_image_point_struct) As Double

Dim d As Double, num As Double
d = Abs((p2.Line - p1.Line) * (p1.Sample - A.Sample) - (p1.Line - A.Line) * (p2.Sample - p1.Sample))
num = Sqr((p2.Line - p1.Line) ^ 2 + (p2.Sample - p1.Sample) ^ 2)
point_line_distance = d / num
End Function


Public Sub affine_coeff(coeff() As Double, A As ads40_image_point_struct, B As ads40_image_point_struct, _
 c As ads40_image_point_struct, ag As point3D, bg As point3D, cg As point3D)


Dim sigmaxp2 As Double, sigmayp2 As Double, sigmaxpyp  As Double, sigmaxp  As Double
Dim sigmayp  As Double, sigmaxpx As Double, sigmaypx As Double
Dim sigmaxpy As Double, sigmaypy As Double, sigmax As Double, sigmay As Double, sigmam As Double
Dim det_A As Double, X As Double, Y As Double, p_x As Double, p_y As Double
Dim j As Integer


ReDim fid_cam_x(1 To 3) As Double
ReDim fid_cam_y(1 To 3) As Double
ReDim fid_ima_x(1 To 3) As Double
ReDim fid_ima_y(1 To 3) As Double

fid_cam_x(1) = ag.X
fid_cam_x(2) = bg.X
fid_cam_x(3) = cg.X
fid_cam_y(1) = ag.Y
fid_cam_y(2) = bg.Y
fid_cam_y(3) = cg.Y

fid_ima_x(1) = A.Line
fid_ima_x(2) = B.Line
fid_ima_x(3) = c.Line
fid_ima_y(1) = A.Sample
fid_ima_y(2) = B.Sample
fid_ima_y(3) = c.Sample

sigmaxp2 = 0:   sigmayp2 = 0:   sigmaxp = 0:   sigmayp = 0
sigmax = 0:   sigmay = 0:   sigmaxpyp = 0:   sigmaxpx = 0
sigmaypx = 0:   sigmaxpy = 0:   sigmaypy = 0
sigmam = CDbl(3)
  
For i = 1 To 3
  sigmaxp2 = sigmaxp2 + fid_cam_x(i) ^ 2
  sigmayp2 = sigmayp2 + fid_cam_y(i) ^ 2
  sigmaxp = sigmaxp + fid_cam_x(i)
  sigmayp = sigmayp + fid_cam_y(i)
  sigmax = sigmax + fid_ima_x(i)
  sigmay = sigmay + fid_ima_y(i)
  sigmaxpyp = sigmaxpyp + fid_cam_x(i) * fid_cam_y(i)
  sigmaxpx = sigmaxpx + fid_cam_x(i) * fid_ima_x(i)
  sigmaypx = sigmaypx + fid_cam_y(i) * fid_ima_x(i)
  sigmaxpy = sigmaxpy + fid_cam_x(i) * fid_ima_y(i)
  sigmaypy = sigmaypy + fid_cam_y(i) * fid_ima_y(i)
  sigmam = CDbl(3)
Next i

Rem solution of a,b, c
ReDim subB(1 To 3, 1 To 3) As Double
ReDim subA(1 To 3, 1 To 3) As Double  ' submatrix
ReDim ATy(1 To 3) As Double

Rem A'y for a,b & c)
ATy(1) = sigmaxpx
ATy(2) = sigmaypx
ATy(3) = sigmax
Rem fill matrix subA  (A'A) upper left block (3 x 3)
subA(1, 1) = sigmaxp2: subA(1, 2) = sigmaxpyp: subA(1, 3) = sigmaxp
subA(2, 1) = sigmaxpyp: subA(2, 2) = sigmayp2: subA(2, 3) = sigmayp
subA(3, 1) = sigmaxp: subA(3, 2) = sigmayp: subA(3, 3) = sigmam

det_A = det_cal3(subA())
For i = 1 To 3: For j = 1 To 3
 subB(i, j) = subA(i, j)
Next j: Next i
For j = 1 To 3
subB(1, j) = ATy(j)
Next j
a_ = 1 / det_A * det_cal3(subB())
'Exit Sub
For i = 1 To 3: For j = 1 To 3
 subB(i, j) = subA(i, j)
Next j: Next i
For j = 1 To 3
subB(2, j) = ATy(j)
Next j
b_ = 1 / det_A * det_cal3(subB())
For i = 1 To 3: For j = 1 To 3
 subB(i, j) = subA(i, j)
Next j: Next i
For j = 1 To 3
subB(3, j) = ATy(j)
Next j
c_ = 1 / det_A * det_cal3(subB())
Rem A'y for parameters (d_, e_, f_)
ATy(1) = sigmaxpy
ATy(2) = sigmaypy
ATy(3) = sigmay
For i = 1 To 3: For j = 1 To 3
 subB(i, j) = subA(i, j)
Next j: Next i
For j = 1 To 3
subB(1, j) = ATy(j)
Next j
d_ = 1 / det_A * det_cal3(subB())
For i = 1 To 3: For j = 1 To 3
 subB(i, j) = subA(i, j)
Next j: Next i
For j = 1 To 3
subB(2, j) = ATy(j)
Next j
e_ = 1 / det_A * det_cal3(subB())

For i = 1 To 3: For j = 1 To 3
 subB(i, j) = subA(i, j)
Next j: Next i
For j = 1 To 3
subB(3, j) = ATy(j)
Next j
f_ = 1 / det_A * det_cal3(subB())

coeff(0) = a_
coeff(1) = b_
coeff(2) = c_
coeff(3) = d_
coeff(4) = e_
coeff(5) = f_


Rem Solution in [a_, b_, c_, d_, e_, f_]
Rem Calculate Residuals and RMSE of both transformations x,y = f(x',y') and x',y' = f(x,y)

End Sub

Public Sub Read_ADS40_SUP_File(FileName As String, j As Long)

Dim LineString As String, dummy As Long
Dim CaseStrings(1 To 44) As String
Rem these are the "key-codes" that precede values
CaseStrings(1) = "IMAGE_ID"
CaseStrings(2) = "IMAGE_FILE_NAME 1 "
CaseStrings(3) = "LINES"
CaseStrings(4) = "SAMPLES"
CaseStrings(5) = "RECTIFICATION_TERMS"
CaseStrings(6) = "GRND_BIAS"
CaseStrings(7) = "BIAS "
CaseStrings(8) = "GROUND_ZERO"
CaseStrings(9) = "LOAD_PT"
CaseStrings(10) = "COORD_SYSTEM"
CaseStrings(11) = "UNITS"
CaseStrings(12) = "USE_FAST_MATH"
CaseStrings(13) = "MAX_AND_MIN_ELEVATION"
CaseStrings(14) = "IMAGE_MOTION"
CaseStrings(15) = "INITIALIZED"
CaseStrings(16) = "STATUS"
CaseStrings(17) = "PHOTO_DATE"
CaseStrings(18) = "QUALITY"
CaseStrings(19) = "SENSOR_TYPE"
CaseStrings(20) = "IMAGE_LEVEL"
CaseStrings(21) = "MEAN_TERRAIN_HEIGHT"
CaseStrings(22) = "ANCHOR_LATITUDE"
CaseStrings(23) = "ANCHOR_LONGITUDE"
CaseStrings(24) = "LINE_STAGGERED"
CaseStrings(25) = "VIEW_OF_LINE"
CaseStrings(26) = "RADIOMETRIC_PROPERTY"
CaseStrings(27) = "IMAGE_DATE"
CaseStrings(28) = "NUMBER_SCAN_LINES"
CaseStrings(29) = "SENSOR_ROTATION "
CaseStrings(30) = "CORRECTION"
CaseStrings(31) = "LEVEL_0_GSD"
CaseStrings(32) = "ORIGINAL_ORIENTATION"
CaseStrings(33) = "ADJUSTED_ORIENTATION"
CaseStrings(34) = "ORIGINAL_CALIBRATION"
CaseStrings(35) = "ADJUSTED_CALIBRATION"
CaseStrings(36) = "LEVEL_0_NAME"
CaseStrings(37) = "RECT_SCALE"
CaseStrings(38) = "RECT_ROTATION"
CaseStrings(39) = "RECT_XOFFSET"
CaseStrings(40) = "RECT_YOFFSET"
CaseStrings(41) = "RECT_HEIGHT"
CaseStrings(42) = "RECT_USE_CALIBRATION"
CaseStrings(43) = "LOCAL_ORIGIN"
CaseStrings(44) = "BASIS"

Open FileName For Input As 222
Do Until EOF(222)
Line Input #222, LineString
 Pos1 = InStr(1, LineString, " ", vbTextCompare)
 Pos2 = InStr(Pos1 + 1, LineString, " ", vbTextCompare)
 Pos3 = InStr(Pos2 + 1, LineString, " ", vbTextCompare)

For i = 1 To 44
pos = InStr(1, LineString, CaseStrings(i), vbTextCompare)
 If pos > 0 Then
  Select Case i
  Case 1
  ADSSUP(j).IMAGE_ID = Mid(LineString, Len(CaseStrings(i)) + 3, Len(LineString) - Len(CaseStrings(i)) - 3)
  Case 2
  ADSSUP(j).IMAGE_FILE_NAME = Mid(LineString, Len(CaseStrings(i)) + 3, Len(LineString) - Len(CaseStrings(i)) - 3)
  Input #222, dummy, ADSSUP(j).LINES
  Input #222, dummy, ADSSUP(j).SAMPLES
  Case 5
  Input #222, ADSSUP(j).RECTIFICATION_TERMS(1), ADSSUP(j).RECTIFICATION_TERMS(2), ADSSUP(j).RECTIFICATION_TERMS(3)
  Input #222, ADSSUP(j).RECTIFICATION_TERMS(4), ADSSUP(j).RECTIFICATION_TERMS(5), ADSSUP(j).RECTIFICATION_TERMS(6)
  Case 6
    ADSSUP(j).GRND_BIAS(1) = Mid(LineString, Pos1, Pos2 - Pos1)
    ADSSUP(j).GRND_BIAS(2) = Mid(LineString, Pos2, Pos3 - Pos2)
    ADSSUP(j).GRND_BIAS(3) = Mid(LineString, Pos3, Len(LineString) - Pos2 - 1)
  Case 7 ' bias and gain
    ADSSUP(j).BIAS = Mid(LineString, Pos1, Pos2 - Pos1)
    ADSSUP(j).GAIN = Mid(LineString, Pos3, Len(LineString) - Pos2)
  Case 8 ' ground zero
    ADSSUP(j).GROUND_ZERO.X = Mid(LineString, Pos1, Pos2 - Pos1)
    ADSSUP(j).GROUND_ZERO.Y = Mid(LineString, Pos2, Pos3 - Pos2)
    ADSSUP(j).GROUND_ZERO.z = Mid(LineString, Pos3, Len(LineString) - Pos2)
  Case 9 ' LOAD_PT
    ADSSUP(j).LOAD_PT.X = Mid(LineString, Pos1, Pos2 - Pos1)
    ADSSUP(j).LOAD_PT.Y = Mid(LineString, Pos2, Pos3 - Pos2)
    ADSSUP(j).LOAD_PT.z = Mid(LineString, Pos3, Len(LineString) - Pos2)
  Case 10 ' coord_SYSTEM
    ADSSUP(j).COORD_SYSTEM = Mid(LineString, Pos1 + 1, Len(LineString) - Pos1)
  Case 11 ' Units
    ADSSUP(j).UNITS = Mid(LineString, Pos1 + 1, Len(LineString) - Pos1)
  Case 12 ' Use fast math
    ADSSUP(j).USE_FAST_MATH = Mid(LineString, Pos1 + 1, Len(LineString) - Pos1)
  Case 13 ' Min max elevation
    ADSSUP(j).MIN_AND_MAX_ELEVATION.X = Mid(LineString, Pos1, Pos2 - Pos1)
    ADSSUP(j).MIN_AND_MAX_ELEVATION.Y = Mid(LineString, Pos2, Len(LineString) - Pos1)
  Case 14 ' ImageMotion
    ADSSUP(j).IMAGE_MOTION = Mid(LineString, Pos1 + 1, Len(LineString) - Pos1)
  Case 15 ' Initialized
    ADSSUP(j).INITIALIZED = Mid(LineString, Pos1 + 1, Len(LineString) - Pos1)
  Case 16 ' status
    ADSSUP(j).Status = Mid(LineString, Pos1 + 1, Len(LineString) - Pos1)
  Case 17 ' photodate
    ADSSUP(j).PHOTO_DATE = Mid(LineString, Pos1 + 2, Len(LineString) - Pos1 - 2)
  Case 18 ' quality
    ADSSUP(j).QUALITY = Mid(LineString, Pos1 + 1, Len(LineString) - Pos1)
  Case 19 ' sensor type
    ADSSUP(j).SENSOR_TYPE = Mid(LineString, Pos1 + 1, Len(LineString) - Pos1)
  Case 20 ' image level
    ADSSUP(j).IMAGE_LEVEL = Mid(LineString, Pos1 + 1, Len(LineString) - Pos1)
  Case 21 ' mean terrain height
    ADSSUP(j).MEAN_TERRAIN_HEIGHT = Mid(LineString, Pos1 + 1, Len(LineString) - Pos1)
  Case 22 ' anchor latitude in radians
    ADSSUP(j).ANCHOR_LATITUDE = Mid(LineString, Pos1 + 1, Len(LineString) - Pos1)
  Case 23 ' anchor longitude in radians
    ADSSUP(j).ANCHOR_LONGITUDE = Mid(LineString, Pos1 + 1, Len(LineString) - Pos1)
  Case 24 ' Line staggered boolean
    ADSSUP(j).LINE_STAGGERED = Mid(LineString, Pos1 + 1, Len(LineString) - Pos1)
  Case 25 ' view of line string
    ADSSUP(j).VIEW_OF_LINE = Mid(LineString, Pos1 + 1, Len(LineString) - Pos1)
  Case 26 ' radiometric property
    ADSSUP(j).RADIOMETRIC_PROPERTY = Mid(LineString, Pos1 + 1, Len(LineString) - Pos1)
  Case 27 ' image date
    ADSSUP(j).IMAGE_DATE = Mid(LineString, Pos1 + 1, Len(LineString) - Pos1)
  Case 28 ' number of scan lines
    ADSSUP(j).NUMBER_SCAN_LINES = Mid(LineString, Pos1 + 1, Len(LineString) - Pos1)
  Case 29 ' sensor rotation boolean
    ADSSUP(j).SENSOR_ROTATION = Mid(LineString, Pos1 + 1, Len(LineString) - Pos1)
  Case 30 ' correction
    ADSSUP(j).CORRECTION = Mid(LineString, Pos1 + 1, Len(LineString) - Pos1)
  Case 31 ' level_0_gsd
    ADSSUP(j).LEVEL_0_GSD = Mid(LineString, Pos1 + 1, Len(LineString) - Pos1)
  Case 32 ' original orientation filename
    ADSSUP(j).ORIGINAL_ORIENTATION = Mid(LineString, Pos1 + 1, Len(LineString) - Pos1)
  Case 33 ' adjusted orientation filename
    ADSSUP(j).ADJUSTED_ORIENTATION = Mid(LineString, Pos1 + 1, Len(LineString) - Pos1)
  Case 34 ' original calibration filename
    ADSSUP(j).ORIGINAL_CALIBRATION = Mid(LineString, Pos1 + 1, Len(LineString) - Pos1)
  Case 35 ' adjusted calibration filename
    ADSSUP(j).ADJUSTED_CALIBRATION = Mid(LineString, Pos1 + 1, Len(LineString) - Pos1)
  Case 36 ' level 0 sup filename
    ADSSUP(j).LEVEL_0_NAME = Mid(LineString, Pos1 + 1, Len(LineString) - Pos1)
  Case 37 ' rect scale
    ADSSUP(j).RECT_SCALE = Mid(LineString, Pos1 + 1, Len(LineString) - Pos1)
  Case 38 ' rect rotation
    ADSSUP(j).RECT_ROTATION = Mid(LineString, Pos1 + 1, Len(LineString) - Pos1)
  Case 39 ' rect xoffset
    ADSSUP(j).RECT_XOFFSET = Mid(LineString, Pos1 + 1, Len(LineString) - Pos1)
  Case 40 ' rect Yoffset
    ADSSUP(j).RECT_YOFFSET = Mid(LineString, Pos1 + 1, Len(LineString) - Pos1)
  Case 41 ' rect Height
    ADSSUP(j).RECT_HEIGHT = Mid(LineString, Pos1 + 1, Len(LineString) - Pos1)
  Case 42 ' rect use calibration boolean
    ADSSUP(j).RECT_USE_CALIBRATION = Mid(LineString, Pos1 + 1, Len(LineString) - Pos1)
  Case 43 ' Local origin in WGS84 XYZ coords
    ADSSUP(j).LOCAL_ORIGIN.X = Mid(LineString, Pos1, Pos2 - Pos1)
    ADSSUP(j).LOCAL_ORIGIN.Y = Mid(LineString, Pos2, Pos3 - Pos2)
    ADSSUP(j).LOCAL_ORIGIN.z = Mid(LineString, Pos3, Len(LineString) - Pos2)
  Case 44 ' BASIS 3x3 rotation matrix, here we need to find 10
   Pos1 = InStr(1, LineString, " ", vbTextCompare)
   Pos2 = InStr(Pos1 + 5, LineString, " ", vbTextCompare)
   Pos3 = InStr(Pos2 + 1, LineString, " ", vbTextCompare)
   Pos4 = InStr(Pos3 + 1, LineString, " ", vbTextCompare)
   Pos5 = InStr(Pos4 + 1, LineString, " ", vbTextCompare)
   Pos6 = InStr(Pos5 + 1, LineString, " ", vbTextCompare)
   Pos7 = InStr(Pos6 + 1, LineString, " ", vbTextCompare)
   Pos8 = InStr(Pos7 + 1, LineString, " ", vbTextCompare)
   Pos9 = InStr(Pos8 + 1, LineString, " ", vbTextCompare)
   Pos10 = InStr(Pos9 + 1, LineString, " ", vbTextCompare)
   ADSSUP(j).BASIS(1, 1) = Mid(LineString, Pos1, Pos2 - Pos1)
   ADSSUP(j).BASIS(1, 2) = Mid(LineString, Pos2, Pos3 - Pos2)
   ADSSUP(j).BASIS(1, 3) = Mid(LineString, Pos3, Pos4 - Pos3)
   ADSSUP(j).BASIS(2, 1) = Mid(LineString, Pos4, Pos5 - Pos4)
   ADSSUP(j).BASIS(2, 2) = Mid(LineString, Pos5, Pos6 - Pos5)
   ADSSUP(j).BASIS(2, 3) = Mid(LineString, Pos6, Pos7 - Pos6)
   ADSSUP(j).BASIS(3, 1) = Mid(LineString, Pos7, Pos8 - Pos7)
   ADSSUP(j).BASIS(3, 2) = Mid(LineString, Pos8, Pos9 - Pos8)
   ADSSUP(j).BASIS(3, 3) = Mid(LineString, Pos9, Len(LineString) - Pos9 + 1)
  
  
  End Select
  
' MsgBox (ADSSUP(j).IMAGE_FILE_NAME)
 End If
Next i

Loop

Close (222)


End Sub


Public Sub Read_ADS40_Cam_File(FileName As String, Cam() As ADS40LineStruct, PixelLoc() As Point2D, i As Long)
On Error GoTo ErrorInReadingADS40CamFile
Open FileName For Input As 222
Dim Cstring As Long, astring As String
Rem We need to have a go-around and define Cstring as long to read the 2nd items correctly per line
Input #222, Cstring, Cam(i).CalibrationFile
Input #222, Cstring, Cam(i).CalibrationSource
Input #222, Cstring, Cam(i).CameraName
Input #222, Cstring, Cam(i).SensorLine
Input #222, Cstring, Cam(i).CalibrationDate
Input #222, Cstring, Cam(i).FocalLength
Cam(i).FocalLength = Cam(i).FocalLength / 1000#
Input #222, Cstring, Cam(i).NumOfPixels
Input #222, Cstring, Cam(i).PixelSize
Input #222, Cstring, Cam(i).RadiometricGain
Input #222, Cstring, Cam(i).IrradianceGain
Input #222, Cstring, Cam(i).LeftSideGain
Input #222, Cstring, Cam(i).PAV_Z_Offset
Input #222, astring
If astring <> "START_XY" Then GoTo ErrorInReadingADS40CamFile
Dim k As Long
Rem In Visual Basic, a static struct cannot exceed 64kBytes and thus the pixel
Rem locations in the focal plane cannot be kept in the same ADS40LineStruct.
For k = 0 To Cam(i).NumOfPixels - 1
 Input #222, PixelLoc(k, i).X, PixelLoc(k, i).Y
 PixelLoc(k, i).X = PixelLoc(k, i).X / 1000#: PixelLoc(k, i).Y = PixelLoc(k, i).Y / 1000#
Next k
Close (222)
Exit Sub
ErrorInReadingADS40CamFile:
apu = MsgBox("There was an error reading the camera file: " & FileName, vbOKOnly, "KUVAMITT Error msg")
Close (222)
End Sub

Public Sub Read_ADS40_ODF_File(FileName As String, i As Long)
Rem Read the EO for the lines, for image i

Open FileName For Binary As 222

Dim cFileId As String * 16
Dim cDataSource As String * 32
Dim cProjectID As String * 64
Dim cStripNameID As String * 64
Dim cNumberOfDataSets As String * 16
Dim cNumOfODFs As Long
Dim cUnitsPosition As String * 2
Dim cPrecisionPosition As String * 14
Dim cUnitsAngles As String * 2
Dim cPrecisionAngles As String * 14
Dim cRotationSequence As String * 16
Dim cAbsoluteTime As String * 16
Dim cComments As String * 96
Dim cAnchorLatitude As String * 16
Dim cAnchorLongitude As String * 16
Dim cBaseTime As String * 12
Dim cPrecisionTime As String * 20
Dim cBaseX As String * 12
Dim cPrecisionStdDevPosition As String * 20
Dim cBaseY As String * 12
Dim cPrecisionStdDevAngles As String * 20
Dim cBaseZ As String * 32

Dim NumOfODFs As Long

Get 222, 1, cFileId
Get 222, 17, cDataSource
Get 222, 49, cProjectID
Get 222, 113, cStripNameID
Get 222, 177, cNumberOfDataSets
Get 222, 193, cUnitsPosition
Get 222, 195, cPrecisionPosition
Get 222, 209, cUnitsAngles
Get 222, 211, cPrecisionAngles
Get 222, 225, cRotationSequence
Get 222, 241, cAbsoluteTime
Get 222, 257, cComments
Get 222, 353, cAnchorLatitude
Get 222, 369, cAnchorLongitude
Get 222, 385, cBaseTime
Get 222, 397, cPrecisionTime
Get 222, 417, cBaseX
Get 222, 429, cPrecisionStdDevPosition
Get 222, 449, cBaseY
Get 222, 461, cPrecisionStdDevAngles
Get 222, 481, cBaseZ

NumOfODFs = Int(Val(Trim(cNumberOfDataSets)))
BaseTime = Int(Val(Trim(cBaseTime)))
PrecisionTime = Int(Val(Trim(cPrecisionTime)))
PrecisionPosition = Int(Val(Trim(cPrecisionPosition)))
BaseX = Int(Val(Trim(cBaseX)))
BaseY = Int(Val(Trim(cBaseY)))
BaseZ = Int(Val(Trim(cBaseZ)))
PrecisionAngles = Int(Val(Trim(cPrecisionAngles)))
PrecisionStdDevPosition = Int(Val(Trim(cPrecisionStdDevPosition)))
PrecisionStdDevAngles = Int(Val(Trim(cPrecisionStdDevAngles)))
Rem StartReading the 40-byte records, from pos 513
Dim ODF As ODFReadstruct, k As Long
ReDim ODFarray(0 To NumOfODFs - 1) As ODFStruct
Dim apu As Long


For k = 0 To NumOfODFs - 1
Get 222, 513 + (k) * 40, ODF

ODFarray(k).GPStime = (ODF.GPStime / PrecisionTime + CDbl(BaseTime))
ODFarray(k).X = ODF.X / CDbl(PrecisionPosition) + CDbl(BaseX)
ODFarray(k).Y = ODF.Y / CDbl(PrecisionPosition) + CDbl(BaseY)
ODFarray(k).z = ODF.z / CDbl(PrecisionPosition) + CDbl(BaseZ)
ODFarray(k).omega = ODF.omega / CDbl(PrecisionAngles)
ODFarray(k).phi = ODF.phi / CDbl(PrecisionAngles)
If Abs(ODFarray(k).phi) > 4 * TO_RADIANS Then
MsgBox ("Found!")
End If

ODFarray(k).kappa = ODF.kappa / CDbl(PrecisionAngles)
Rem the reading of unsigned short -values fails and the STDDEV-values are incorrect
ODFarray(k).StdDevX = ODF.StdDevX / CDbl(PrecisionStdDevPosition)
ODFarray(k).StdDevY = ODF.StdDevY / CDbl(PrecisionStdDevPosition)
ODFarray(k).StdDevZ = ODF.StdDevZ / CDbl(PrecisionStdDevPosition)
ODFarray(k).StdDevOmega = ODF.StdDevOmega / CDbl(PrecisionStdDevAngles)
ODFarray(k).StdDevPhi = ODF.StdDevPhi / CDbl(PrecisionStdDevAngles)
ODFarray(k).StdDevKappa = ODF.StdDevKappa / CDbl(PrecisionStdDevAngles)
Next k
Close (222)

Rem This code uses a c-dll that converts the unsigned data into signed int (VB long)
length = Len(FileName)
ReDim filename_in(0 To length) As Byte
For j = 0 To length - 1
  filename_in(j) = CByte(Asc(Mid$(FileName, j + 1, 1)))
Next j
filename_in(length) = 0
ReDim STDDEVARRAY(1 To 6 * NumOfODFs) As Long
k = ADS40_READODFFILE(NumOfODFs, filename_in(0), STDDEVARRAY(1))
Form1.Cls

For k = 0 To NumOfODFs - 1
 ODFs(k, i).GPStime = ODFarray(k).GPStime
 ODFs(k, i).X = ODFarray(k).X
 ODFs(k, i).Y = ODFarray(k).Y
 ODFs(k, i).z = ODFarray(k).z
 ODFs(k, i).omega = ODFarray(k).omega
 ODFs(k, i).phi = ODFarray(k).phi
 ODFs(k, i).kappa = ODFarray(k).kappa
 
 ODFs(k, i).a00 = Cos(ODFarray(k).phi) * Cos(ODFarray(k).kappa)
 ODFs(k, i).a10 = -Cos(ODFarray(k).phi) * Sin(ODFarray(k).kappa)
 ODFs(k, i).a20 = Sin(ODFarray(k).phi)
 ODFs(k, i).a01 = Cos(ODFarray(k).omega) * Sin(ODFarray(k).kappa) + Sin(ODFarray(k).omega) * Sin(ODFarray(k).phi) * Cos(ODFarray(k).kappa)
 ODFs(k, i).a11 = Cos(ODFarray(k).omega) * Cos(ODFarray(k).kappa) - Sin(ODFarray(k).omega) * Sin(ODFarray(k).phi) * Sin(ODFarray(k).kappa)
 ODFs(k, i).a21 = -Sin(ODFarray(k).omega) * Cos(ODFarray(k).phi)
 ODFs(k, i).a02 = Sin(ODFarray(k).omega) * Sin(ODFarray(k).kappa) - Cos(ODFarray(k).omega) * Sin(ODFarray(k).phi) * Cos(ODFarray(k).kappa)
 ODFs(k, i).a12 = Sin(ODFarray(k).omega) * Cos(ODFarray(k).kappa) + Cos(ODFarray(k).omega) * Sin(ODFarray(k).phi) * Sin(ODFarray(k).kappa)
 ODFs(k, i).a22 = Cos(ODFarray(k).omega) * Cos(ODFarray(k).phi)

 
 ODFs(k, i).StdDevX = STDDEVARRAY((k) * 6 + 1) / CDbl(PrecisionStdDevPosition)
 ODFs(k, i).StdDevY = STDDEVARRAY((k) * 6 + 2) / CDbl(PrecisionStdDevPosition)
 ODFs(k, i).StdDevZ = STDDEVARRAY((k) * 6 + 3) / CDbl(PrecisionStdDevPosition)
 ODFs(k, i).StdDevOmega = STDDEVARRAY((k) * 6 + 4) / CDbl(PrecisionStdDevAngles)
 ODFs(k, i).StdDevPhi = STDDEVARRAY((k) * 6 + 5) / CDbl(PrecisionStdDevAngles)
 ODFs(k, i).StdDevKappa = STDDEVARRAY((k) * 6 + 6) / CDbl(PrecisionStdDevAngles)
 Form1.PSet (k / 40 + 20, 500 - ODFs(k, i).omega * 3000)
 Form1.PSet (k / 40 + 20, 500 - ODFs(k, i).phi * 3000)
Next k


End Sub

Public Sub Read_ADS40_ADS_File(FileName As String, j As Long)
 
Dim LineString As String, dummy As Long
Dim CaseStrings(1 To 44) As String
Rem these are the "key-codes" that precede values
CaseStrings(1) = "ADS_HEADER"
CaseStrings(2) = "BANDS"
CaseStrings(3) = "DEPTH"
CaseStrings(4) = "BITS"
CaseStrings(5) = "LINES"
CaseStrings(6) = "SAMPLES"
CaseStrings(7) = "TILE_X"
CaseStrings(8) = "TILE_Y"
CaseStrings(9) = "HARDWARE_COMPRESSED"
CaseStrings(10) = "LINES_PER_BLOCK"
CaseStrings(11) = "SAMPLES_PER_BLOCK"
CaseStrings(12) = "BLOCK_DATA"


 

Open FileName For Input As 222
Do Until EOF(222)
Line Input #222, LineString
 Pos1 = InStr(1, LineString, " ", vbTextCompare)
 Pos2 = InStr(Pos1 + 1, LineString, " ", vbTextCompare)
 Pos3 = InStr(Pos2 + 1, LineString, " ", vbTextCompare)
 
 For i = 1 To 12
 pos = InStr(1, LineString, CaseStrings(i), vbTextCompare)
 If pos > 0 Then
  Select Case i
  Case 1 ' ADS_HEADER
  ADSHDR(j).HEADER = Mid(LineString, Pos1, Len(LineString) - Pos1 + 1)
  Case 2
  ADSHDR(j).BANDS = Mid(LineString, Pos1, Len(LineString) - Pos1 + 1)
  Case 3
  ADSHDR(j).DEPTH = Mid(LineString, Pos1, Len(LineString) - Pos1 + 1)
  Case 4
  ADSHDR(j).BITS = Mid(LineString, Pos1, Len(LineString) - Pos1 + 1)
  Case 5
  ADSHDR(j).LINES = Mid(LineString, Pos1, Len(LineString) - Pos1 + 1)
  Case 6
  ADSHDR(j).SAMPLES = Mid(LineString, Pos1, Len(LineString) - Pos1 + 1)
  Case 7
  ADSHDR(j).TILE_X = Mid(LineString, Pos1, Len(LineString) - Pos1 + 1)
  Case 8
  ADSHDR(j).TILE_Y = Mid(LineString, Pos1, Len(LineString) - Pos1 + 1)
  Case 9
  ADSHDR(j).HARDWARE_COMPRESSED = Mid(LineString, Pos1, Len(LineString) - Pos1 + 1)
  Case 10
  ADSHDR(j).LINES_PER_BLOCK = Mid(LineString, Pos1, Len(LineString) - Pos1 + 1)
  Case 11
  ADSHDR(j).SAMPLES_PER_BLOCK = Mid(LineString, Pos1, Len(LineString) - Pos1 + 1)
  Case 12
  ADSHDR(j).BLOCK_DATA(j, 1).L0 = Mid(LineString, 21, 1)
  ADSHDR(j).BLOCK_DATA(j, 1).L1 = Mid(LineString, 23, 1)
  ADSHDR(j).BLOCK_DATA(j, 1).FileName = Mid(LineString, 25, Len(LineString) - 25 + 1)
  
    
  End Select
 End If
Next i
 
Loop


Close (222)

End Sub

Public Function grnd2lp(j As Long, gp As point3D, lp As ads40_image_point_struct) As Long
Rem Ground point to Line/position

Rem   imgPt                  -> the last used image point
Rem   curRot                 -> rotation matrix (elements a00 -> a22)
Rem   num_scanlines          -> number of scan lines in the image
Rem   data                   -> data object that holds all meta information (including focal length)
Rem   calibpix_per_line      -> number of pixels per scan line

Dim lp1 As ads40_image_point_struct, lp2 As ads40_image_point_struct, p1 As ads40_image_point_struct
Dim p2 As ads40_image_point_struct, a_focal As ads40_image_point_struct, p1_focal As ads40_image_point_struct
Dim p2_focal As ads40_image_point_struct
Dim cand As Long, i As Long, start_line As Long, end_line As Long, N As Long
N = ADSSUP(j).NUMBER_SCAN_LINES
Dim dist As Double, mindist As Double
mindist = 10000000000#
Dim c As Double
c = ADSCam(j).FocalLength
Dim gp_new As point3D
gp_new.X = gp.X: gp_new.Y = gp.Y: gp_new.z = gp.z

' get coarse window first
apu = GetSearchWindow(j, gp_new, lp1, lp2)
' and then do the sequential search

   start_line = Int(lp1.Line)
   end_line = Int(lp2.Line)
   
   cand = Int((start_line + end_line) / 2)
   p1.Line = Int((start_line + end_line) / 2)
   p2.Line = Int((start_line + end_line) / 2)
   p1.Sample = lp1.Sample
   p2.Sample = lp2.Sample
    
   ' image to focal (Image2Focal.c)

   apu = lp2f(j, p1, p1_focal)
   apu = lp2f(j, p2, p2_focal)
    
   For i = start_line To end_line
   
      ' ground to focal (Ground2Focal.c)

      apu = grnd2f(j, gp, i, a_focal)
      dist = point_line_distance(a_focal, p1_focal, p2_focal)
      If (dist < mindist) Then
         cand = i
         mindist = dist
       End If
   Next i
 
   ' and the final transformation
   apu = grnd2f(j, gp, cand, a_focal)
  ' store the current image point (in line/position !!)

   If (f2lp(j, a_focal, cand, lp)) Then
    grnd2lp = -1
    Exit Function
   End If
   
   imgPt(j).Sample = lp.Sample
   imgPt(j).Line = lp.Line
   'PrevPoint.X = gp.X
   'PrevPoint.Y = gp.Y
   'PrevPoint.z = gp.z
   grnd2lp = 1

  If mindist > 0.00002 Then grnd2lp = 0


End Function
Public Function f2lp(j As Long, f As ads40_image_point_struct, Line As Long, lp As ads40_image_point_struct) As Long

 Dim start As Long
 start = 0:
 Dim end_ As Long
 end_ = ADSCam(j).NumOfPixels - 1
 Dim pos As Double, yp As Double
 yp = f.Line

 pos = start + (end_ - start) * (yp - ADSPixels(start, j).Y) / (ADSPixels(end_, j).Y - ADSPixels(start, j).Y)

   If (pos >= 0# And pos < (ADSCam(j).NumOfPixels - 1)) Then
      
      If ((pos - 20#) < 0) Then
       start = Int(0#)
      Else
       start = Int(pos - 20#)
      End If
      
      If (pos + 20#) > end_ Then
       end_ = Int(end_)
      Else
       end_ = Int(pos + 20#)
      End If
      
      pos = start + (end_ - start) * (yp - ADSPixels(start, j).Y) / (ADSPixels(end_, j).Y - ADSPixels(start, j).Y)
   End If

   lp.Line = CDbl(Line)
   lp.Sample = pos

   f2lp = 0

End Function


Public Function lp2Grnd(i As Long, lp As ads40_image_point_struct, gp As point3D) As Long

Rem Level0 to ground
Dim apu As Long
Dim f As ads40_image_point_struct
apu = lp2f(i, lp, f)
apu = f2grnd(i, f, lp.Line, gp)


End Function

Public Function grnd2f(j As Long, gp As point3D, ByVal Line As Long, xy As ads40_image_point_struct) As Long

  Dim Index As Long, N As Long, pos As Long
  N = ADSSUP(j).NUMBER_SCAN_LINES
  Dim opk As point3D
  Dim c As Double
  c = ADSCam(j).FocalLength
  Dim A As Double, B As Double, N_ As Double, dX As Double, dY As Double, dZ As Double
  Dim axy As ads40_image_point_struct

  Index = Line

' check whether we're outside image content
' in case we are compute the the positions outside the
' image content so we have smooth cursor roaming
   
   If (Line >= 0 And Line < N) Then
   
      dX = gp.X - ODFs(Line, j).X
      dY = gp.Y - ODFs(Line, j).Y
      dZ = gp.z - ODFs(Line, j).z

' we are outside the image !!
   Else
      If (Line >= N) Then
         Index = N - 1
         dX = gp.X - (ODFs(N - 1, j).X + (Line - N) * (ODFs(N - 1, j).X - ODFs(N - 2, j).X))
         dY = gp.Y - (ODFs(N - 1, j).Y + (Line - N) * (ODFs(N - 1, j).Y - ODFs(N - 2, j).Y))
         dZ = gp.z - (ODFs(N - 1, j).z + (Line - N) * (ODFs(N - 1, j).z - ODFs(N - 2, j).z))
      ElseIf (Line < 0) Then
         Index = 0

         dX = gp.X - (ODFs(0, j).X + Line * (ODFs(1, j).X - ODFs(2, j).X))
         dY = gp.Y - (ODFs(0, j).Y + Line * (ODFs(1, j).Y - ODFs(2, j).Y))
         dY = gp.z - (ODFs(0, j).z + Line * (ODFs(1, j).z - ODFs(2, j).z))
      End If
      
   End If

' collinearity equations

   A = ODFs(Index, j).a00 * dX + ODFs(Index, j).a01 * dY + ODFs(Index, j).a02 * dZ
   B = ODFs(Index, j).a10 * dX + ODFs(Index, j).a11 * dY + ODFs(Index, j).a12 * dZ
   N_ = ODFs(Index, j).a20 * dX + ODFs(Index, j).a21 * dY + ODFs(Index, j).a22 * dZ

' focal coordinates
    
    xy.Sample = -c * A / N_
    xy.Line = -c * B / N_
    
    grnd2f = 0

End Function

Public Function GetSearchWindow(i As Long, gp As point3D, lp1 As ads40_image_point_struct, lp2 As ads40_image_point_struct) As Long

Dim A As ads40_image_point_struct, B As ads40_image_point_struct
Dim c As ads40_image_point_struct, tmp As ads40_image_point_struct
Dim ag As point3D, bg As point3D, cg As point3D
Dim dist As Double, win_width As Double, win_height As Double
Dim coeff(0 To 5) As Double
    
   ag.z = gp.z
   bg.z = gp.z
   cg.z = gp.z
    
   dist_limit = 1000
   
   dist = 10000 ' (PrevPoint.X - gp.X) * (PrevPoint.X - gp.X) _
        '+ (PrevPoint.Y - gp.Y) * (PrevPoint.Y - gp.Y) _
        '+ (PrevPoint.z - gp.z) * (PrevPoint.z - gp.z)

    tmp.Line = imgPt(i).Line     ' the last "used" image point (in line/position) !!
    tmp.Sample = imgPt(i).Sample

    If (dist > dist_limit) Then  ' check the (squared) distance in object space !!
        ' TODO:
        '      - lp should be changed to im (much better and general approach !!
        A.Line = 0: A.Sample = 0: B.Line = 0
        B.Sample = ADSCam(i).NumOfPixels - 1
        c.Line = ADSHDR(i).LINES - 1
        c.Sample = ADSCam(i).NumOfPixels / 2
        win_width = B.Sample - A.Sample + 1
        win_height = c.Line - A.Line + 1

        Do While (win_width > DEF_WINDOW_WIDTH Or win_height > DEF_WINDOW_HEIGHT)
       
           apu = lp2Grnd(i, A, ag)
           aa = ag.X: aa = ag.Y: aa = ag.z
           aa = A.Line: aa = A.Sample
           
           apu = lp2Grnd(i, B, bg)
           aa = B.Line: aa = B.Sample
           aa = bg.X: aa = bg.Y: aa = bg.z
           
           apu = lp2Grnd(i, c, cg)
           aa = c.Line: aa = c.Sample
           aa = cg.X: aa = cg.Y: aa = cg.z
    
           Call affine_coeff(coeff, A, B, c, ag, bg, cg)
           
            
            tmp.Sample = coeff(5) + coeff(3) * gp.X + coeff(4) * gp.Y
            tmp.Line = coeff(2) + coeff(0) * gp.X + coeff(1) * gp.Y

            A.Line = tmp.Line - win_height / 4#
            B.Line = tmp.Line - win_height / 4#
            c.Line = tmp.Line + win_height / 4#
            A.Sample = tmp.Sample - win_width / 4#
            B.Sample = tmp.Sample + win_width / 4#
            c.Sample = tmp.Sample
         
            win_width = B.Sample - A.Sample
            win_height = c.Line - A.Line
         
         Loop
        
      lp1.Line = tmp.Line - DEF_WINDOW_HEIGHT / 2
      lp1.Sample = tmp.Sample - DEF_WINDOW_WIDTH / 2

      lp2.Line = tmp.Line + DEF_WINDOW_HEIGHT / 2
      lp2.Sample = tmp.Sample + DEF_WINDOW_WIDTH / 2
    ElseIf (dist < dist_limit) Then
                  ' this point has not moved far, so we can keep the search space small !!
    
        ' move these constants into #defines...
        
        lp1.Line = imgPt(i).Line - 30
        lp1.Sample = imgPt(i).Sample - 200
        
        lp2.Line = imgPt(i).Line + 30
        lp2.Sample = imgPt(i).Sample + 200
    End If

   GetSearchWindow = 0

End Function


Public Function f2grnd(ByVal j As Long, xy As ads40_image_point_struct, ByVal Line As Long, ByRef gp_out As point3D) As Long


Dim i As Long, N As Long, indx As Long
i = Line
N = ADSSUP(j).NUMBER_SCAN_LINES

Dim c As Double
c = ADSCam(j).FocalLength

Dim x0 As Double, y0 As Double, z0 As Double
Dim opk As point3D
Dim A As Double, B As Double, N_ As Double
Dim axy As ads40_image_point_struct

    axy.Line = xy.Line
    axy.Sample = xy.Sample
    indx = i

   If (i < 0) Then indx = 0
   If (i >= N) Then indx = N - 1

   ' camera position, j refers to image

   x0 = ODFs(indx, j).X
   y0 = ODFs(indx, j).Y
   z0 = ODFs(indx, j).z

   ' check whether we're outside image content
   ' in case we are shift the camera position appropriate distance so
   ' we always get a solution for this transformation

   If (i < 0) Then
      x0 = x0 + i * (ODFs(indx + 1, j).X - x0)
      y0 = y0 + i * (ODFs(indx + 1, j).Y - y0)
      z0 = y0 + i * (ODFs(indx + 1, j).z - z0)
   ElseIf (i >= N) Then
      x0 = x0 + (i - N) * (x0 - ODFs(indx - 1, j).X)
      y0 = y0 + (i - N) * (y0 - ODFs(indx - 1, j).Y)
      z0 = z0 + (i - N) * (z0 - ODFs(indx - 1, j).z)
   End If

   ' and here are the actual collinearity equations

   A = ODFs(indx, j).a00 * axy.Sample + ODFs(indx, j).a10 * axy.Line - ODFs(indx, j).a20 * c
   B = ODFs(indx, j).a01 * axy.Sample + ODFs(indx, j).a11 * axy.Line - ODFs(indx, j).a21 * c
   N_ = gp_out.z - z0
   N_ = N_ / (ODFs(indx, j).a02 * axy.Sample + ODFs(indx, j).a12 * axy.Line - ODFs(indx, j).a22 * c)

   gp_out.X = x0 + A * N_
   gp_out.Y = y0 + B * N_
    
   f2grnd = 0

End Function

Public Function lp2f(i As Long, lp As ads40_image_point_struct, f As ads40_image_point_struct) As Long

Rem Level 0 to Focal Plane

Dim pos As Long, d As Double, nc As Long
pos = CLng(lp.Sample)
d = lp.Sample - pos
nc = ADSCam(i).NumOfPixels - 1

If pos < 0# Then
  f.Line = ADSPixels(0, i).Y - pos * (ADSPixels(0, i).Y - ADSPixels(1, i).Y)
  f.Sample = ADSPixels(0, i).X
  lp2f = 0
  Exit Function
ElseIf pos >= nc Then
  f.Line = ADSPixels(nc, i).Y - (pos - nc) * (ADSPixels(nc - 1, i).Y - ADSPixels(nc, i).Y)
  f.Sample = ADSPixels(nc, i).X
  lp2f = 0
  Exit Function
End If
  f.Sample = ADSPixels(pos, i).X + d * (ADSPixels(pos + 1, i).X - ADSPixels(pos, i).X)
  f.Line = ADSPixels(pos, i).Y + d * (ADSPixels(pos + 1, i).Y - ADSPixels(pos, i).Y)
  lp2f = 0
End Function

Public Sub ADS_SOLVE_XYZ_from_Z_x_y(Index As Long, Line As Double, Z_gnd As Double, X_Gnd As Double, Y_Gnd As Double, X As Double, Y As Double)


If Z_gnd < 0 Then Z_gnd = 0

Dim c As Double ' camera constant
Dim k As Double ' denominator in inverse collinear eq.
Dim z As Double

c = -ADSCam(Index).FocalLength

Rem INVERSE IMAGE TO GROUND NEEDS A TRANSPOSE
a00 = ODFs(Line, Index).a00
a10 = ODFs(Line, Index).a01
a20 = ODFs(Line, Index).a02
a01 = ODFs(Line, Index).a10
a11 = ODFs(Line, Index).a11
a21 = ODFs(Line, Index).a12
a02 = ODFs(Line, Index).a20
a12 = ODFs(Line, Index).a21
a22 = ODFs(Line, Index).a22

Rem The position of the principal point
x0 = ODFs(Line, Index).X
y0 = ODFs(Line, Index).Y
z0 = ODFs(Line, Index).z

Rem Coordinates to be solved (Z is fixed)
'x = X_Gnd
'Y = Y_Gnd
z = Z_gnd

Rem Solution using collinear equations
k = a20 * X + a21 * Y + a22 * c
If k <> 0# Then
 X_Gnd = x0 + (z - z0) * ((a00 * (X) + a01 * (Y) + a02 * (c)) / k)
 Y_Gnd = y0 + (z - z0) * ((a10 * (X) + a11 * (Y) + a12 * (c)) / k)
End If
End Sub


Public Sub UTM_to_KKJ(kkj_x As Double, kkj_y As Double, kkj_z As Double, UTM_E As Double, UTM_N As Double, H As Double)
Rem Local, Hyytiälä spesific set of eqns, based on a triangle of geodetic points +- 5 cm accuracy
kkj_x = -0.04614190132 * UTM_N + 0.9990901664 * UTM_E + 2474938.474
kkj_y = 0.9990949702 * UTM_N + 0.04615302083 * UTM_E - 10341.1406
kkj_z = H - 18.67 ' FIN2000 geoid model difference in the area
End Sub
Public Sub KKJ_to_UTM(kkj_x As Double, kkj_y As Double, kkj_z As Double, UTM_E As Double, UTM_N As Double, H As Double)
Rem Local, Hyytiälä spesific set of eqns, based on a triangle of geodetic points +- 5 cm accuracy
UTM_E = -2471441.562 + 0.9987798071 * kkj_x + 0.04612734592 * kkj_y
UTM_N = 124518.3273 - 0.04613846192 * kkj_x + 0.9987750048 * kkj_y
H = kkj_z + 18.67
End Sub

Public Sub LSR_to_WGS84XYZ(i As Long, xn As Double, yn As Double, Zn As Double, Xwgs As Double, Ywgs As Double, Zwgs As Double)
Rem Perform LSR (local cartesian) to WGS84-XYZ conversion
Xwgs = ADSSUP(i).BASIS(1, 1) * xn + ADSSUP(i).BASIS(2, 1) * yn + ADSSUP(i).BASIS(3, 1) * Zn + ADSSUP(i).LOCAL_ORIGIN.X
Ywgs = ADSSUP(i).BASIS(1, 2) * xn + ADSSUP(i).BASIS(2, 2) * yn + ADSSUP(i).BASIS(3, 2) * Zn + ADSSUP(i).LOCAL_ORIGIN.Y
Zwgs = ADSSUP(i).BASIS(1, 3) * xn + ADSSUP(i).BASIS(2, 3) * yn + ADSSUP(i).BASIS(3, 3) * Zn + ADSSUP(i).LOCAL_ORIGIN.z
End Sub

Public Sub KKJ_to_LSR(i As Long, kkj_x As Double, kkj_y As Double, kkj_z As Double, Xl As Double, yl As Double, zl As Double)

Dim Lat As Double, Lon As Double, H As Double, UTM_E As Double, UTM_N As Double
Rem First KKJ to UTM
Call KKJ_to_UTM(kkj_x, kkj_y, kkj_z, UTM_E, UTM_N, H)
Rem UTM to WGS84
Dim apu As Long, Zone As Long
Zone = 35
Dim xn As Double, yn As Double, Zn As Double
apu = UTM_TO_WGS84XYZ(xn, yn, Zn, Lat, Lon, H, UTM_E, UTM_N, Zone)
Rem Perform LSR (local cartesian) to WGS84-XYZ conversion
Xl = ADSSUP(i).BASIS(1, 1) * (xn - ADSSUP(i).LOCAL_ORIGIN.X) + ADSSUP(i).BASIS(1, 2) * (yn - ADSSUP(i).LOCAL_ORIGIN.Y) + ADSSUP(i).BASIS(1, 3) * (Zn - ADSSUP(i).LOCAL_ORIGIN.z)
yl = ADSSUP(i).BASIS(2, 1) * (xn - ADSSUP(i).LOCAL_ORIGIN.X) + ADSSUP(i).BASIS(2, 2) * (yn - ADSSUP(i).LOCAL_ORIGIN.Y) + ADSSUP(i).BASIS(2, 3) * (Zn - ADSSUP(i).LOCAL_ORIGIN.z)
zl = ADSSUP(i).BASIS(3, 1) * (xn - ADSSUP(i).LOCAL_ORIGIN.X) + ADSSUP(i).BASIS(3, 2) * (yn - ADSSUP(i).LOCAL_ORIGIN.Y) + ADSSUP(i).BASIS(3, 3) * (Zn - ADSSUP(i).LOCAL_ORIGIN.z)

End Sub
Public Sub LSR_TO_KKJ(i As Long, Xl As Double, yl As Double, zl As Double, kkj_x As Double, kkj_y As Double, kkj_z As Double)
Dim Xwgs As Double, Ywgs As Double, Zwgs As Double
Call LSR_to_WGS84XYZ(i, Xl, yl, zl, Xwgs, Ywgs, Zwgs)
Dim apu As Long, Lat As Double, Lon As Double, H As Double, UTM_E As Double, UTM_N As Double, Zone As Long
Zone = 35
apu = WGS84XYZ_TO_UTM(Xwgs, Ywgs, Zwgs, Lat, Lon, H, UTM_E, UTM_N, Zone)
Call UTM_to_KKJ(kkj_x, kkj_y, kkj_z, UTM_E, UTM_N, H)
End Sub


Public Sub ADS_get_surface_height_click()



Rem Point a target in image(0)
Rem Routine computes the path of the ray, ssovles for XYZ, when Z takes steps around the current solution
Rem Routine captures a template in image 0 around the image position
Rem For al points XYZ along the camerra ray of the image(0), the template is checked for correlation with
Rem image(1). Maximum correlation r -1,1 occurs at certain XYZ position, which is made the 3D solution

Dim Index As Integer, Z_gnd As Double, X_Gnd As Double, Y_Gnd As Double
Dim p_x As Double, p_y As Double
Dim R_tau(1 To 300, 4) As Double, Rmaxind As Long
Dim Z_cent As Double
Rem Always use the image(0) upper-left, i.e. set index to zero

'On Error GoTo Error_In_Get_Surface_Height
Index = 0
Z_cent = Z_sol
Rem If there is no solution, start looking around 160 m a.s.l.
If SolutionExists = False Then Z_cent = Z_sol
Rem Assign the test variable Z_gnd with initial value, center of the search in Z
Z_gnd = Z_cent
Rem Solve XY for the given Z and image observation of left image, here Z is converted to LSR
Z_gnd = Z_gnd + 18.67
Call ADS_SOLVE_XYZ_from_Z_x_y(CLng(Index), ADSOBS(0).lp.Line, Z_gnd, X_Gnd, Y_Gnd, ADSOBS(0).xy.X, ADSOBS(0).xy.Y)

Rem SOlve position in the pixel coordinates
Dim lp As ads40_image_point_struct
Dim fp As ads40_image_point_struct
Dim gp As point3D
Dim kkj_x As Double, kkj_y As Double, kkj_z As Double
gp.X = X_Gnd: gp.Y = Y_Gnd: gp.z = Z_gnd
apu = grnd2lp(CLng(Index), gp, lp)

Call LSR_TO_KKJ(0, gp.X, gp.Y, gp.z, kkj_x, kkj_y, kkj_z)



aa = lp.Line
aa = lp.Sample

Rem Get pixels of the reference template. bkuva() -RGBtriplet array
Rem *** SECTION FOR FILLING THE TEMPLATE bkuva() WITH PIXEL-values, starts here **

Close (2)
Open image_info(Index).FileName For Binary As 2
 ReDim RROW(0 To (TempWidth - 1)) As Integer
 startrow = lp.Sample - TempWidth / 2
 startcol = lp.Line - image_info(Index).o_col - TempWidth / 2
 endcol = startcol + (TempWidth - 1)
 ReDim bkuva(0 To (TempWidth - 1), 0 To (TempWidth - 1)) As Byte
k = -1
 For i = startrow To startrow + (TempWidth - 1)
   k = k + 1
   paikka = CLng(i) * CLng(image_info(Index).sub_width) * 2 + CLng(startcol) * 2
   Rem Read a row
   Get #2, paikka + 1, RROW()
    kX = 0
    For m = 0 To (endcol - startcol)
      bkuva(m, k) = CByte(RROW(m) / 20)
   Next m
 Next i

Close (2)

Open "c:\temp\bkuva.raw" For Binary As 20
Put 20, , bkuva
Close (20)

Rem *** SECTION FOR FILLING THE TEMPLATE, bkuva() WITH PIXEL-values, stops here here **
Rem Check the texture in the tepmplate, only if interesting, we compute
Dim meanA As Double, nim1 As Double
meanA = 0
nim1 = 0
 For i = 0 To UBound(bkuva, 1)
   For j = 0 To UBound(bkuva, 2)
    meanA = meanA + bkuva(i, j)
   Next j
 Next i
 meanA = meanA / ((UBound(bkuva, 1) + 1) ^ 2)
  
 For i = 0 To UBound(bkuva, 1)
   For j = 0 To UBound(bkuva, 2)
   nim1 = nim1 + (CDbl(bkuva(i, j)) - meanA) * (CDbl(bkuva(i, j)) - meanA)
   Next j
 Next i

Dim CV As Double
Rem Coefficient of VAR
CV = Sqr(nim1 / ((UBound(bkuva, 1) + 1) ^ 2 - 1)) / meanA
'Exit Sub
If CV < CV_limit Then GoTo Nextpoint
Rem Now we should search for a reference template in image(1) "right" at positions along the image ray
Rem of image(0) "left"
Dim Z_test As Double
Rem We let Z vary +- Z_depth around the initial value
Rem It is image(1) that is the target of template matching
Index = 1
Rem Discretize the image ray by solving for a XYZ position at every Z_step distance in Z

Dim Ix As Long, Rmax As Double
Rem ix is a counter for XYZ points, Rmax is set at -5
Ix = 0
Rmax = -5
'Z_gnd = Z_cent
Rem Start to discretize the image ray of image(0)
For Z_test = Z_gnd - Z_depth To Z_gnd + Z_depth Step Z_step
  Ix = Ix + 1
  Rem Solve pixel location
  gp.z = Z_test
  apu = grnd2lp(CLng(Index), gp, lp)
  aa = lp.Line
  aa = lp.Sample
  
  Rem *** GET PIXELS OF IMAGE1 into ckuva() aRGBtriplet array ***
  Open image_info(Index).FileName For Binary As 2
  startrow = lp.Sample - TempWidth / 2
  startcol = lp.Line - image_info(Index).o_col - TempWidth / 2
  endcol = startcol + (TempWidth - 1)
  
  ReDim ckuva(0 To (TempWidth - 1), 0 To (TempWidth - 1)) As Byte
  k = -1
  For i = startrow To startrow + (TempWidth - 1)
   k = k + 1
   paikka = CLng(i) * CLng(image_info(Index).sub_width) * 2 + CLng(startcol) * 2
   Rem Read a row
   Get #2, paikka + 1, RROW()
    For m = 0 To (endcol - startcol)
      ckuva(m, k) = CByte(RROW(m) / 20)
    Next m
   Next i
  
  Open "c:\temp\test_" & Format$(gp.z, "0.00") & ".raw" For Binary As 20
  Put #20, , ckuva
  Close (20)
  
  Close (2)
  Rem *** GET PIXELS OF IMAGE1 into ckuva() a RGBtriplet array, done ***
  Rem r is the correlation coefficient, computed for tempates bkuva() (fixed) and ckuva() (centered at XYZ in image(1)
  Dim r As Double
  rx = MYFUNC_CCORA(bkuva(0, 0), ckuva(0, 0), r, CDbl(meanA), CLng(TempWidth))
  Rem Check if this is the maximal correlation so far
  If r > Rmax Then
   Rmaxind = Ix
   Rmax = r
  End If
  R_tau(Ix, 1) = r
  'Exit Sub
  R_tau(Ix, 2) = X_Gnd
  R_tau(Ix, 3) = Y_Gnd
  R_tau(Ix, 4) = Z_test
Next Z_test

Rem After checking all positions, assign 3D solution with point that had largest correlation
X_sol = R_tau(Rmaxind, 2)
Y_sol = R_tau(Rmaxind, 3)
Z_sol = R_tau(Rmaxind, 4)
SolutionExists = True
Rem Update Main-form
'Z_sol = Z_sol - 18.67
Call LSR_TO_KKJ(CLng(0), X_sol, Y_sol, Z_sol, gp.X, gp.Y, gp.z)
X_sol = gp.X: Y_sol = gp.Y: Z_sol = gp.z


Form1.Label4(0).Caption = Format$(X_sol, "#.00")
Form1.Label4(1).Caption = Format$(Y_sol, "#.00")
Form1.Label4(2).Caption = Format$(Z_sol, "#.00")
Rem Update measurement -structure
Measurement.num = MeasurementCounter
Measurement.X = X_sol: Measurement.Y = Y_sol: Measurement.z = Z_sol
Rem RMSE is now correlation
Measurement.rmse = R_tau(Rmaxind, 1)
Rem Get DTM-elevation

Measurement.Z_dtm = getheight(X_sol, Y_sol)


Open ImageMatchingOutPutFile For Append As 6
'Open "c:\temp\Stand2_InShadow.txt" For Append As 6
If R_tau(Rmaxind, 1) > Rlim Then
 Print #6, X_sol & "," & Y_sol & "," & Z_sol & "," & Measurement.Z_dtm
End If
Close (6)
Call LSR_TO_KKJ(CLng(0), X_sol, Y_sol, Z_sol, gp.X, gp.Y, gp.z)
Exit Sub
Call center_to_xyz

For i = 0 To 1
If R_tau(Rmaxind, 1) > Rlim Then
Call r_transform_ground_to_pixel(i, R_tau(Rmaxind, 2), R_tau(Rmaxind, 3), R_tau(Rmaxind, 4), p_x, p_y)
Form1.Picture1(i).DrawWidth = 3
'Form1.Picture1(i).Cls
'Form1.Picture1(i).PSet ((p_x - (image_info(i).o_col + win_info(i).win_o_col)) * win_info(i).pan_x - 1, ((image_info(i).Height - 1) - p_y - (image_info(i).o_row + win_info(i).win_o_row)) * win_info(i).pan_y), RGB(225, 255, 25)
Form1.Picture1(i).FontSize = 12
Form1.Picture1(i).FontBold = True

Form1.Picture1(i).ForeColor = RGB(225, 255, 25)
Form1.Picture1(i).CurrentX = Form1.Picture1(i).CurrentX + 15
Form1.Picture1(i).CurrentY = Form1.Picture1(i).CurrentY + 15
Form1.Picture1(i).Print Format$(R_tau(Rmaxind, 1), "R=0.00") & " " & Format$(R_tau(Rmaxind, 4), "Z=#.00")
'Form1.Picture1(i).PSet ((p_x - (image_info(i).o_col + win_info(i).win_o_col)) * win_info(i).pan_x, ((image_info(i).Height - 1) - p_y - (image_info(i).o_row + win_info(i).win_o_row)) * win_info(i).pan_y), RGB(225, 255, 25)
Form1.Picture1(i).CurrentX = Form1.Picture1(i).CurrentX + 15
Form1.Picture1(i).CurrentY = Form1.Picture1(i).CurrentY + 35
Form1.Picture1(i).Print "DTM: " & Format$(Measurement.Z_dtm, "#.00 m")
Form1.Picture1(i).DrawWidth = 1
End If
Next i

Call GetLiDARObs


Nextpoint:

Exit Sub
Error_In_Get_Surface_Height:
Close (2)
Close (6)
Close (7)
MsgBox ("An error occurred - resuming...")
End Sub

Public Sub R_transform_ground_to_ADS40(i As Long, ObjectX As Double, ObjectY As Double, ObjectZ As Double, lp As ads40_image_point_struct)
 Dim gp As point3D
 Call KKJ_to_LSR(CLng(i), ObjectX, ObjectY, ObjectZ, gp.X, gp.Y, gp.z)
 apu = grnd2lp(CLng(i), gp, lp)
End Sub