C *****************************DLTMAC******************************* C ****************************************************************** C * * C * DIRECT LINEAR TRANSFORMATION --- DLT * C * THIS PROGRAM SOLVES THE COLLINEARITY EQUATIONS OF CONVENTIONAL * C * PHOTOGRAMMETRY BY THE DIRECT LINEAR TRANSFORMATION METHOD. * C * IT IS A MODIFICATION OF THE PROGRAM FOR THE DLT METHOD WITH * C * DISTORTION PARAMETERS PROGRAMMED BY MARZAN AND KARARA - 1975. * C * THE EQUATIONS ARE SOLVED DIRECTLY WITHOUT ITERATION,WITHOUT * C * INITIAL APPROXIMATIONS FOR THE UNKNOWNS AND WITHOUT THE NEED * C * FOR FIDUCIAL MARKS,SINCE THE SOLUTION GOES DIRECTLY FROM * C * COMPARATOR COORDINATES TO OBJECT SPACE COORDINATES. * C * DEPENDING ON THE AMOUNT OF CONTROL AVAILABLE,UP TO 16 UNKNOWNS * C * CAN BE OBTAINED FROM THE SOLUTION : * C * 11 DLT PARAMETERS IN WHICH LINEAR FILM DEFORMATION AND LENS * C * DISTORTION ARE IMPLICIT * C * 12 DLT PARAMETERS PLUS THE COEFFICIENT OF THE 3RD ORDER TERM * C * OF SYMMETRICAL LENS DISTORTION * C * 14 DLT PARAMETERS PLUS THE COEFFICIENTS OF THE 3RD,5TH,7TH * C * ORDER TERMS OF SYMMETRICAL LENS DISTORTION * C * 16 DLT PARAMETERS PLUS THREE COEFFICIENTS OF SYMMETRICAL LENS * C * DISTORTION PLUS THE COEFFICIENTS OF THE FIRST TWO TERMS OF * C * DECENTERING LENS DISTORTION * C * OBSERVATIONS FROM THREE TYPES OF COMPARATORS CAN BE USED ----- * C * MONO ... X AND Y COORDINATES ON ONE PHOTOGRAPH * C * STEREO 1 ... X AND Y COORDINATES ON LEFT PHOTO AND PX AND PY * C * STEREO 2 ... X AND Y COORDINATES ON LEFT PHOTO AND X AND Y * C * COORDINATES ON RIGHT PHOTO * C * * C ****************************************************************** C ****************************************************************** C * * C * PROGRAM INPUT * C * ******* ***** * C * * C * CARD FORMAT COLUMN DATA * C * ---- ------ ------ ---- * C * * C * 1 FREE 1-80 ALPHANUMERIC CHARACTERS DESCRIBING THE JOB * C * * C * 2 5I5 1- 5 NPHOT:NUMBER OF PHOTOS USED (UP TO 4)* * C * 6-10 NPAIR:NUMBER OF STEREO-PAIRS * C * 11-15 NCONT:NUMBER OF CONTROL POINTS (UP TO 60)* * C * 16-20 NKNOWN:NUMBER OF KNOWN COORDINATED POINTS * C * CONTROL + CHECK (UP TO 60)* * C * * C * 3 FREE 1-80 FORMAT OF CONTROL POINT INFORMATION * C * * C * 4... AS 3 AS 3 CONTROL POINT NUMBER,XYZ COORDINATES OF * C * CONTROL POINT,SX SY SZ STANDARD ERROR OF * C * CONTROL POINT COORDINATES * C * ONE KNOWN POINT PER CARD * C * * C * ************************* * C * * C * THIS BLOCK IS REPEATED * C * PER PHOTOGRAPH OR PER * C * STEREO-PAIR * C * * C * CARD FORMAT COLUMN DATA * C * ---- ------ ------ ---- * C C * 5 5I5 1- 5 NTYPE:CODE NUMBER FOR TYPE OF COMPARATOR * C * 6-10 NPOINT:NUMBER OF PHOTO POINTS OBSERVED * C * (UP TO 150)* * C * 11-15 NREP:NUMBER OF REPITITIONS OF PHOTO POINT * C * OBSERVATIONS * C * * C * 6 5I5 1- 5 PHOTO NUMBER (MONO-COMPARATOR) * C * 1- 5 PHOTO NUMBER LEFT * C * 6-10 PHOTO NUMBER RIGHT (STEREO-COMPARATOR) * C * * C * 7 FREE 1-80 FORMAT OF COMPARATOR INFORMATION * C * * C * 8... AS 7 AS 7 PHOTO POINT NUMBER,PHOTO POINT COORDINATES * C * NTYPE=0 XY PHOTO COORDINATES * C * NTYPE=1 XY PHOTO COORDINATES ON LEFT PHOTO * C * PLUS PX AND PY * C * NTYPE=2 XY PHOTO COORDINATES ON LEFT PHOTO * C * AND XY PHOTO COORDINATES ON RIGHT PHOTO * C * ONE PHOTO POINT PER CARD AND ONE CARD PER * C * REPETITION OF OBSERVATIONS * C * * C * ************************* * C * * C * CARD FORMAT COLUMN DATA * C * ---- ------ ------ ---- * C C * 9 5I5 1- 5 NUMBER OF GROUPS OF UNKNOWNS TO BE SOLVED * C * (1-4) * C * * C * 10-13 5I5 1- 5 NUMBER OF PARAMETERS TO BE SOLVED FOR * C * 11,12,14,16. ONE NUMBER PER CARD - ORDER OF * C * CARDS DECIDES WHICH GROUPS ARE SOLVED FOR * C * * C * * THESE LIMITS APPLY ONLY TO THIS VERSION OF THE PROGRAM. * C * LARGER OR SMALLER LIMITS MAY BE SET BY ALTERING THE DECLARED * C * DIMENSIONS OF THE RELEVANT MATRICES. * C * * C ****************************************************************** C C DECLARATION STATEMENTS C *********** ********** C IMPLICIT REAL*8 (A-H,O-Z) DIMENSION JOB(20),FMT1(10),FMT2(10),FMT3(10) DIMENSION Q(20),SD(20),A(4),DF(4),SVV(4),ICOUNT(4) COMMON/DATA/ X(150,4),Y(150,4),E(60,3),NUM(60),SP(4),NC(4),NNP, $NPHOT,NKNOWN COMMON/TRANS/ W(20,20),S(60,3),D(4,20),XP(4),YP(4),IPHOT(4) COMMON/NUMBER/ NP(150,4),NPC(4),NCONT,NCON,NCHK,NPT C C ****************************************************************** C ***********************INPUT DATA********************************* C ****************************************************************** C DO 5 I=1,4 ICOUNT(I)=0 SVV(I)=0.0 5 CONTINUE K=0 CREAD JOB HEADING READ(5,999) (JOB(I),I=1,20) WRITE(6,902) JOB C CREAD NPHOT NUMBER OF PHOTOGRAPHS C NPAIR NUMBER OF STEREO-PAIRS C NCONT NUMBER OF CONTROL POINTS C NKNOWN NUMBER OF KNOWN POINTS - NCONT +CHECK POINTS READ(5,900) NPHOT,NPAIR,NCONT,NKNOWN C C INPUT CONTROL DATA C ***** ******* **** C CREAD CONTROL POINT NUMBER,XYZ CONTROL POINT COORDINATES,SX SY SZ C STANDARD ERROR OF CONTROL POINT COORDINATES READ(5,998) (FMT1(I),I=1,10) DO 10 I=1,NKNOWN READ(5,FMT1) NUM(I),(E(I,J),J=1,3),(S(I,J),J=1,3) 10 CONTINUE C CWRITE CONTROL POINT NUMBERS,COORDINATES AND STANDARD ERRORS WRITE(6,903) WRITE(6,904) DO 15 I=1,NCONT WRITE(6,905) NUM(I),(E(I,J),J=1,3),(S(I,J),J=1,3) 15 CONTINUE C CWRITE CHECK POINT NUMBERS,COORDINATES AND STANDARD ERRORS IF(NCONT.EQ.NKNOWN) GO TO 100 WRITE(6,906) WRITE(6,904) M=NCONT+1 DO 20 I=M,NKNOWN WRITE(6,905) NUM(I),(E(I,J),J=1,3),(S(I,J),J=1,3) 20 CONTINUE C C INPUT COMPARATOR OBSERVATIONS OF PHOTO POINTS C ***** ********** ************ ** ***** ****** C C C HAVE COMPARATOR COORDINATES OF ALL PHOTOGRAPHS BEEN READ IN ? 100 IF(K.GE.NPHOT) GO TO 105 C CREAD NTYPE CODE FOR TYPE OF COMPARATOR OBSERVATIONS C NPOINT NUMBER OF POINTS OBSERVED IN PHOTO OR STEREO-PAIR C NREP NUMBER OF REPITITIONS OF OBSERVATIONS READ(5,900) NTYPE,NPOINT,NREP C C NTYPE = 0 ... MONO-COMPARATOR X,Y C NTYPE = 1 ... STEREO-COMPARATOR XL,YL,PX,PY C NTYPE = 2 ... STEREO-COMPARATOR XL,YL,XR,YR C Z=1 IF(NTYPE-1) 110,115,120 C C MONO-COMPARATOR OBSERVATIONS : NTYPE = 0 110 CONTINUE K=K+1 NPC(K)=NPOINT C CALCULATE THE DEGREES OF FREEDOM OF THE COMPARATOR OBSERVATIONS DF(K)=NREP*(NREP-1) C CREAD PHOTO NUMBER READ(5,900) IPHOT(K) CREAD PHOTO POINT NUMBER AND X Y COMPARATOR COORDINATES READ(5,998) (FMT2(I),I=1,10) DO 25 I=1,NPOINT X(I,K)=0.0 Y(I,K)=0.0 VV=0.0 DO 30 J=1,NREP READ(5,FMT2) NO,XC,YC ICOUNT(K)=ICOUNT(K)+1 X(I,K)=X(I,K)+XC Y(I,K)=Y(I,K)+YC VV=VV+XC*XC+YC*YC 30 CONTINUE NP(I,K)=NO VV=VV-(X(I,K)**2+Y(I,K)**2)/NREP SVV(K)=SVV(K)+DABS(VV) C FORM MEAN COMPARATOR COORDINATES OF PHOTO POINTS X(I,K)=X(I,K)/NREP Y(I,K)=Y(I,K)/NREP 25 CONTINUE C C CALCULATE THE STANDARD ERROR OF THE COMPARATOR OBSERVATIONS C FIXED DIVIDE BY ZERO ERROR - DAS FOR HABROUK 97/06/20 IF(NREP.GT.1) SP(K)=DSQRT(SVV(K)/(2.0*DF(K)*ICOUNT(K))) IF(NREP.EQ.1) SP(K)=0.002 C CWRITE THE MEAN OBSERVED COMPARATOR COORDINATES AND THEIR STANDARD ERROR WRITE(6,907) WRITE(6,908) IPHOT(K) DO 35 J=1,NPOINT WRITE(6,909) NP(J,K),X(J,K),Y(J,K) 35 CONTINUE WRITE(6,910) SP(K) GO TO 100 C C STEREO-COMPARATOR OBSERVATIONS 115 Z=0 120 CONTINUE K=K+2 L=K-1 M=K NPC(L)=NPOINT NPC(M)=NPOINT SVV(L)=0.0 SVV(M)=0.0 C CALCULATE THE DEGREES OF FREEDOM OF THE COMPARATOR OBSERVATIONS DF(M)=NREP*(NREP-1) DF(L)=NREP*(NREP-1) C CREAD PHOTO POINT NUMBERS READ(5,900) IPHOT(L),IPHOT(M) CREAD PHOTO POINT NUMBER,XL YL COMPARATOR COORDINATES AND EITHER PX PY C OR XR YR COMPARATOR COORDINATES READ(5,998) (FMT3(I),I=1,10) DO 40 I=1,NPOINT X(I,L)=0.0 Y(I,L)=0.0 X(I,M)=0.0 Y(I,M)=0.0 VVL=0.0 VVM=0.0 DO 45 J=1,NREP READ(5,FMT3) NO,XC,YC,PX,PY ICOUNT(L)=ICOUNT(L)+1 ICOUNT(M)=ICOUNT(M)+1 IF(Z.EQ.0) GO TO 130 C NTYPE = 2 XL,YL,XR,YR XD=PX YD=PY GO TO 135 130 CONTINUE C NTYPE = 1 XL,YL,PX,PY XD=XC-PX YD=YC-PY 135 CONTINUE X(I,L)=X(I,L)+XC Y(I,L)=Y(I,L)+YC X(I,M)=X(I,M)+XD Y(I,M)=Y(I,M)+YD VVL=VVL+XC*XC+YC*YC VVM=VVM+XD*XD+YD*YD 45 CONTINUE NP(I,L)=NO NP(I,M)=NO SUML=X(I,L)**2+Y(I,L)**2 SUMM=X(I,M)**2+Y(I,M)**2 VVL=VVL-SUML/NREP VVM=VVM-SUMM/NREP SVV(L)=SVV(L)+DABS(VVL) SVV(M)=SVV(M)+DABS(VVM) C FORM MEAN COMPARATOR COORDINATES OF PHOTO POINTS X(I,L)=X(I,L)/NREP Y(I,L)=Y(I,L)/NREP X(I,M)=X(I,M)/NREP Y(I,M)=Y(I,M)/NREP 40 CONTINUE C C CALCULATE THE STANDARD ERROR OF THE COMPARATOR OBSERVATIONS IF(NREP.EQ.1) GO TO 41 SP(L)=DSQRT(SVV(L)/(2.0*DF(L)*ICOUNT(L))) SP(M)=DSQRT(SVV(M)/(2.0*DF(M)*ICOUNT(M))) GO TO 42 41 SP(L)=0.002 SP(M)=0.002 42 CONTINUE C CWRITE THE MEAN OBSERVED COMPARATOR COORDINATES AND THEIR STANDARD ERROR WRITE(6,907) WRITE(6,911) IPHOT(L),IPHOT(M) WRITE(6,912) DO 50 J=1,NPOINT WRITE(6,913) NP(J,L),X(J,L),Y(J,L),NP(J,M),X(J,M),Y(J,M) 50 CONTINUE WRITE(6,914) SP(L),SP(M) GO TO 100 C 105 CONTINUE C C ****************************************************************** C ************************CALIBRATION******************************* C ****************************************************************** C C PARAMETER DETERMINATION FOR EACH PHOTOGRAPH C ********* ************* *** **** ********** C CREAD NUMBER OF GROUPS OF UNKNOWNS TO BE SOLVED READ(5,900) NIP DO 700 ISET=1,NIP WRITE(6,915) CREAD NUMBER OF UNKNOWNS READ(5,900) IP WRITE(6,916) IP IF(IP.GE.11) WRITE(6,917) IF(IP.EQ.12) WRITE(6,918) IF(IP.GE.14) WRITE(6,919) IF(IP.GE.16) WRITE(6,920) C DO 55 L=1,NPHOT C FIND ALL CONTROL POINTS IN PHOTO L AND ORGANISE DATA CALL SORT1(L) C NNC=NC(L) IPP1=IP+1 IPM1=IP-1 IF(IP.LT.13) GO TO 140 IPM2=IP-2 IF(IP.EQ.14) GO TO 140 IPM3=IP-3 IPM4=IP-4 140 CONTINUE C C CALCULATE DEGREES OF FREEDOM OF SOLUTION DEGF=2.0*NNC-IP IF(DEGF.EQ.0) DEGF=1.0 C XP(L)=0.0 YP(L)=0.0 C XP,YP = COMPARATOR COORDINATES OF PRINCIPAL POINT OF PHOTO L C C DO TWO ITERATIONS.FIRST ITERATION HAS NO VALUES FOR XP YP,WEIGHT, C OR DIVISOR 'A'.SECOND ITERATION USES VALUES OF THE UNKNOWNS C OBTAINED FROM THE FIRST ITERATION TO EVALUATE XP YP,WEIGHT AND 'A' C WHICH ARE USED TO GIVE 'FINAL' ANSWER. C DO 60 LL=1,2 VV=0.0 C INITIALISE NORMAL EQUATIONS DO 65 I=1,IP DO 65 J=1,IPP1 W(I,J)=0.0 65 CONTINUE C DO 70 K=1,NNC IF(IP.EQ.11) GO TO 145 C FORM COEFFICIENTS OF LENS DISTORTION PARAMETERS XR=X(K,L)-XP(L) YR=Y(K,L)-YP(L) R1=XR**2 R3=YR**2 R2=R1+R3 IF(IP.EQ.12) GO TO 145 R4=R2*R2 R6=R2*R4 IF(IP.EQ.14) GO TO 145 R7=2.0*R1+R2 R8=2.0*R3+R2 R9=2.0*XR*YR 145 CONTINUE C C FORM CONDITION EQUATIONS FOR X AND Y DO 75 III=1,2 C INITIALISE CONDITION EQUATIONS DO 80 I=1,15 Q(I)=0.0 80 CONTINUE C C INITIAL VALUES FOR WEIGHT AND DIVISOR 'A' AB=1.0 A(L)=1.0 C IF(LL.EQ.1) GO TO 150 C REVISED VALUE OF DIVISOR 'A' DO 85 I=1,3 A(L)=A(L)+D(L,I+8)*E(K,I) 85 CONTINUE C REVISED VALUE OF WEIGHT GO TO (155,160),III 155 B1=(D(L,9)*X(K,L)-D(L,1)) B2=(D(L,10)*X(K,L)-D(L,2)) B3=(D(L,11)*X(K,L)-D(L,3)) GO TO 165 160 B1=(D(L,9)*Y(K,L)-D(L,5)) B2=(D(L,10)*Y(K,L)-D(L,6)) B3=(D(L,11)*Y(K,L)-D(L,7)) 165 CONTINUE AB=(A(L)*SP(L))**2+(B1*S(K,1))**2+(B2*S(K,2))**2+(B3*S(K,3))**2 AB=DSQRT(AB) C 150 CONTINUE C GO TO (170,175),III C FORM X CONDITION EQUATION 170 DO 90 I=1,3 Q(I)=E(K,I) Q(I+8)=-1.0*E(K,I)*X(K,L) 90 CONTINUE Q(4)=10.0 Q(IPP1)=X(K,L) IF(IP.EQ.11) GO TO 180 IF(IP.EQ.12) GO TO 185 IF(IP.EQ.14) GO TO 190 Q(IPM4)=XR*R2*A(L) Q(IPM3)=XR*R4*A(L) Q(IPM2)=XR*R6*A(L) Q(IPM1)=R7*A(L) Q(IP)=R9*A(L) GO TO 180 190 CONTINUE Q(IPM2)=XR*R2*A(L) Q(IPM1)=XR*R4*A(L) Q(IP)=XR*R6*A(L) GO TO 180 185 Q(IP)=XR*R2*A(L) GO TO 180 C C FORM Y CONDITION EQUATION 175 DO 95 I=1,3 Q(I+4)=E(K,I) Q(I+8)=-1.0*E(K,I)*Y(K,L) 95 CONTINUE Q(8)=10.0 Q(IPP1)=Y(K,L) IF(IP.EQ.11) GO TO 180 IF(IP.EQ.12) GO TO 195 IF(IP.EQ.14) GO TO 200 Q(IPM4)=YR*R2*A(L) Q(IPM3)=YR*R4*A(L) Q(IPM2)=YR*R6*A(L) Q(IPM1)=R9*A(L) Q(IP)=R8*A(L) GO TO 180 200 CONTINUE Q(IPM2)=YR*R2*A(L) Q(IPM1)=YR*R4*A(L) Q(IP)=YR*R6*A(L) GO TO 180 195 Q(IP)=YR*R2*A(L) 180 CONTINUE C C APPLY WEIGHT TO CONDITION EQUATION DO 210 I=1,IPP1 Q(I)=Q(I)/AB 210 CONTINUE C C COMPUTE CONTRIBUTION OF CONDITION EQUATION TO SUM OF RESIDUALS C SQUARED AND OBTAIN CUMULATIVE SUM VV=VV+Q(IPP1)*Q(IPP1) C C FORM NORMAL EQUATIONS DO 215 I=1,IP DO 215 J=I,IPP1 W(I,J)=W(I,J)+Q(I)*Q(J) 215 CONTINUE DO 220 I=1,IPM1 KL=I+1 DO 220 J=KL,IP W(J,I)=W(I,J) 220 CONTINUE 75 CONTINUE 70 CONTINUE C DO 225 I=1,IP Q(I)=W(I,IPP1) 225 CONTINUE C C TEST FOR SINGULARITY OF NORMAL EQUATION TOL=1.D-6 C SOLVE NORMAL EQUATIONS CALL SWPMAT(W,1,IP,IPP1,KERR,TOL) C C OBTAIN VALUES OF THE UNKNOWNS AND COMPUTE CONTRIBUTION OF NORMAL C EQUATIONS TO THE SUM OF RESIDUALS SQUARED,OBTAIN TOTAL SUM DO 230 I=1,IP D(L,I)=W(I,IPP1) VV=VV-Q(I)*W(I,IPP1) 230 CONTINUE C C COMPUTE COORDINATES OF PRINCIPAL POINT AND PRINCIPAL DISTANCE FOR C PHOTOGRAPH L CALL XPYPC(L,LL) C IF(LL.EQ.1) GO TO 60 C C COMPUTE AND PRINT STANDARD ERROR OF UNIT WEIGHT VV=DABS(VV/DEGF) STD=DSQRT(VV) WRITE(6,921) STD 60 CONTINUE C C COMPUTE VARIANCE/COVARIANCE MATRIX OF COMPUTED VALUES OF THE C UNKNOWNS DO 235 I=1,IP DO 240 J=1,IP W(I,J)=W(I,J)*VV 240 CONTINUE SD(I)=DSQRT(W(I,I)) 235 CONTINUE C CWRITE UNKNOWNS AND THEIR STANDARD ERRORS WRITE(6,922) IPHOT(L) WRITE(6,923) CWRITE DLT PARAMETERS DO 245 I=1,11 WRITE(6,924) D(L,I),SD(I) 245 CONTINUE IF(IP.EQ.11) GO TO 55 CWRITE LENS DISTORTION PARAMETERS WRITE(6,925) DO 250 I=12,IP WRITE(6,924) D(L,I),SD(I) 250 CONTINUE 55 CONTINUE C C *************************INTERSECT******************************** C C SOLVE FOR OBJECT SPACE COORDINATES OF PHOTO POINTS C ***** *** ****** ***** *********** ** ***** ****** C C FIND ALL POINTS THAT LIE IN THE OVERLAP COMMON TO ALL PHOTOS C ORGANISE DATA CALL SORT2 C SOLVE FOR OBJECT SPACE COORDINATES CALL OBJECT(IPM4,IPM3,IPM2,IPM1,IP) 700 CONTINUE C 900 FORMAT(5I5) 902 FORMAT(T1,'1',T5,'JOB : ',20A4) 903 FORMAT(//,' ',T27,'--- OBJECT SPACE COORDINATES OF CONTROL POINTS $---',//) 904 FORMAT(T5,'(POINT)',10X,'(X)',12X,'(Y)',12X,'(Z)',12X,'(SX)',6X,'( $SY)',6X,'(SZ)',//) 905 FORMAT(T5,I7,3F13.5,4X,3F8.5) 906 FORMAT(//,' ',T28,'--- OBJECT SPACE COORDINATES OF CHECK POINTS -- $-',//) 907 FORMAT(///,T25,'--- OBSERVED COMPARATOR COORDINATES OF PHOTO POINT $S ---') 908 FORMAT(//,T5,'PHOTO',I4,//,T5,'(POINT)',10X,'(X)',12X,'(Y)',//) 909 FORMAT(T5,I7,2(7X,F8.3)) 910 FORMAT(/,T2,'-----------------------------------------',//,T2,'(RM $S OF COMPARATOR OBSERVATIONS) ... ',F6.3) 911 FORMAT(//,T5,'PHOTO',I4,48X,'PHOTO',I4) 912 FORMAT(//,T5,'(POINT)',10X,'(X)',12X,'(Y)',22X,'(POINT)',10X,'(X)' $,12X,'(Y)',//) 913 FORMAT(T5,I7,2(7X,F8.3),20X,I7,2(7X,F8.3)) 914 FORMAT(/,T2,'----------------------------------------------------- $----------------------------------------------',//,T2,'(RMS COMPAR $ATOR OBSERVATIONS ... LEFT : ',F9.6,' RIGHT : ',F9.6) 915 FORMAT(////,T25,'--- PARAMETER DETERMINATION FOR EACH PHOTOGRAPH-- $-',/) 916 FORMAT(/,T5,'NUMBER OF PARAMETERS =',I4) 917 FORMAT(T5,'11 DLT PARAMETERS') 918 FORMAT(T5,'FIRST COEFFICIENT OF RADIAL LENS DISTORTION POLYNOMIAL $K1') 919 FORMAT(T5,'FIRST THREE COEFFICIENTS OF RADIAL LENS DISTORTION POLY $NOMIAL K1,K2,K3') 920 FORMAT(T5,'FIRST TWO COEFFICIENTS OF DECENTERING LENS DISTORTION P $1,P2') 921 FORMAT(/,T5,'STANDARD ERROR OF UNIT WEIGHT =',F10.4) 922 FORMAT(/,T5,'PHOTO ',I4) 923 FORMAT(/,T5,'DLT PARAMETERS',5X,'STANDARD ERRORS',/) 924 FORMAT(2D18.6) 925 FORMAT(//,T5,'LENS DISTORTION',4X,'STANDARD ERRORS',/,T8,'PARAMETE $RS',/) 998 FORMAT(10A8) 999 FORMAT(20A4) C STOP END C C SUBROUTINE SORT1(K) C ********** ******** C ****************************************************************** C * THIS SORT ROUTINE FINDS THOSE CONTROL POINTS WHICH HAVE BEEN * C * OBSERVED IN PHOTO K,THEN ORGANISES THE STACKING OF THE CONTROL * C * POINT DATA AND PHOTO POINT DATA FOR PHOTO K SO THAT A CONTROL * C * POINT AND ITS CORRESPONDING COMPARATOR COORDINATES ARE MATCHED * C ****************************************************************** C IMPLICIT REAL*8 (A-H,O-Z) DIMENSION L(60) COMMON/DATA/ X(150,4),Y(150,4),E(60,3),NUM(60),SP(4),NC(4),NNP, $NPHOT,NKNOWN COMMON/NUMBER/ NP(150,4),NPC(4),NCONT,NCON,NCHK,NPT C C INITIALISE CONTROL POINT COUNTERS NC(K)=0 NNC=0 JJ=NCONT+1 M=NPC(K) C C FOR EACH CONTROL POINT FIND ITS CORRESPONDING PHOTO POINT DO 5 I=1,NCONT 110 CONTINUE DO 10 J=1,M IF(NUM(I).NE.NP(J,K)) GO TO 10 C CONTROL POINT MATCHED SO SAVE LOCATION OF PHOTO POINT L(I)=J NC(K)=NC(K)+1 NNC=NNC+1 GO TO 100 10 CONTINUE C C CONTROL POINT NOT OBSERVED IN THIS PHOTO SO MOVE IT TO END OF C STACK AND BRING NEW CONTROL POINT TO BEGINNING FOR MATCHING JJ=JJ-1 IF(JJ.EQ.NNC)GO TO 105 NTEMP=NUM(JJ) XTEMP=E(JJ,1) YTEMP=E(JJ,2) ZTEMP=E(JJ,3) NUM(JJ)=NUM(I) E(JJ,1)=E(I,1) E(JJ,2)=E(I,2) E(JJ,3)=E(I,3) NUM(I)=NTEMP E(I,1)=XTEMP E(I,2)=YTEMP E(I,3)=ZTEMP GO TO 110 C C MOVE PHOTO POINT TO SAME STACK POSITION AS CONTROL POINT 100 CONTINUE J=L(I) IF(J.EQ.I) GO TO 5 NTEMP=NP(J,K) XTEMP=X(J,K) YTEMP=Y(J,K) NP(J,K)=NP(I,K) X(J,K)=X(I,K) Y(J,K)=Y(I,K) NP(I,K)=NTEMP X(I,K)=XTEMP Y(I,K)=YTEMP 5 CONTINUE 105 CONTINUE RETURN END C C SUBROUTINE XPYPC(L,LL) C ********** *********** C ****************************************************************** C * THIS SUBROUTINE GIVES APPROXIMATIONS FOR THE COMPARATOR COORD- * C * INATES OF THE PRINCIPAL POINT AND FOR THE PRINCIPAL DISTANCE * C * OF PHOTO L * C ****************************************************************** C IMPLICIT REAL*8 (A-H,O-Z) DIMENSION Q(5) COMMON/TRANS/ W(20,20),S(60,3),D(4,20),XP(4),YP(4),IPHOT(4) C C INITIALISE COEFFICIENT VECTOR DO 5 I=1,5 Q(I)=0.0 5 CONTINUE C C FORM COEFFICIENT VECTOR DO 10 I=1,3 J=I+8 K=I+4 Q(1)=Q(1)+D(L,I)*D(L,J) Q(2)=Q(2)+D(L,K)*D(L,J) Q(3)=Q(3)+D(L,I)*D(L,I) Q(4)=Q(4)+D(L,K)*D(L,K) Q(5)=Q(5)+D(L,J)*D(L,J) 10 CONTINUE C C SOLVE FOR PRINCIPAL POINT COMPARATOR COORDINATES XP(L)=Q(1)/Q(5) YP(L)=Q(2)/Q(5) C SOLVE FOR SCALED PRINCIPAL DISTANCES IN X AND Y DIRECTIONS CX =Q(3)/Q(5)-XP(L)*XP(L) CY =Q(4)/Q(5)-YP(L)*YP(L) C SOLVE FOR PRINCIPAL DISTANCE CX=DSQRT(CX) CY=DSQRT(CY) C=(CX+CY)*0.5 C CWRITE BASIC INTERIOR ORIENTATION PARAMETERS IF(LL.EQ.1) GO TO 100 WRITE(6,900) IPHOT(L) WRITE(6,901) XP(L),YP(L),C 100 CONTINUE C 900 FORMAT(/,T5,'INTERIOR ORIENTATION, PHOTO',I4,/) 901 FORMAT(T5,'XP=',F10.4,5X,'YP=',F10.4,5X,'C=',F10.4) C RETURN END C C SUBROUTINE SORT2 C ********** ***** C ****************************************************************** C * THIS SORT ROUTINE FINDS PHOTO POINTS WHICH HAVE BEEN OBSERVED * C * IN ALL PHOTOGRAPHS.IT IDENTIFIES THEM AS PHOTO POINTS OF * C * CONTROL,CHECK OR UNKNOWN OBJECT POINTS AND ORGANISES THE PHOTO * C * POINTS OF THE CONTROL AND CHECK POINTS TO THE SAME STACK * C * POSITIONS AS THE CONTROL AND CHECK POINTS - CONTROL FIRST * C * FOLLOWED BY CHECK.UNKNOWN OBJECT POINTS FOLLOW IN RANDOM ORDER * C ****************************************************************** C IMPLICIT REAL*8 (A-H,O-Z) DIMENSION CONT(60,3),CHEK(60,3),POINT(150,3),P(4) COMMON/DATA/ X(150,4),Y(150,4),E(60,3),NUM(60),SP(4),NC(4),NNP, $NPHOT,NKNOWN COMMON/NUMBER/ NP(150,4),NPC(4),NCONT,NCON,NCHK,NPT C C INITIALISE COUNTERS M=NPC(1) KK=M+1 NNP=0 NCON=0 NCHK=0 NPT=0 C C FOR EACH PHOTO POINT IN PHOTO 1 FIND THE CORRESPONDING PHOTO POINT C IN ALL OTHER PHOTOS DO10 I=1,M 110 CONTINUE ICOUNT=0 DO 15 J=2,NPHOT N=NPC(J) DO 20 K=1,N IF(NP(I,1).NE.NP(K,J)) GO TO 20 ICOUNT=ICOUNT+1 GO TO 15 20 CONTINUE 15 CONTINUE C C DOES THE PHOTO POINT APPEAR IN ALL PHOTOS ? IF(ICOUNT.EQ.(NPHOT-1)) GO TO 100 C C PHOTO POINT DOES NOT APPEAR ON ALL PHOTOS SO MOVE IT TO END OF C STACK AND TRY A NEW PHOTO POINT KK=KK-1 IF(KK.EQ.NNP) GO TO 105 NTEMP=NP(KK,1) XTEMP=X(KK,1) YTEMP=Y(KK,1) NP(KK,1)=NP(I,1) X(KK,1)=X(I,1) Y(KK,1)=Y(I,1) NP(I,1)=NTEMP X(I,1)=XTEMP Y(I,1)=YTEMP GO TO 110 C C PHOTO POINT APPEARS ON ALL PHOTOS 100 CONTINUE NNP=NNP+1 C C IS PHOTO POINT A CONTROL POINT ? DO 25 II=1,NCONT IF(NUM(II).NE.NP(I,1)) GO TO 25 NCON=NCON+1 CONT(NCON,1)=NP(I,1) CONT(NCON,2)=X(I,1) CONT(NCON,3)=Y(I,1) GO TO 10 25 CONTINUE C C IS PHOTO POINT A CHECK POINT ? IF(NCONT.EQ.NKNOWN) GO TO 115 NN=NCONT+1 DO 30 II=NN,NKNOWN IF(NUM(II).NE.NP(I,1)) GO TO 30 NCHK=NCHK+1 CHEK(NCHK,1)=NP(I,1) CHEK(NCHK,2)=X(I,1) CHEK(NCHK,3)=Y(I,1) GO TO 10 30 CONTINUE C C PHOTO POINT IS AN UNKNOWN OBJECT POINT 115 CONTINUE NPT=NPT+1 POINT(NPT,1)=NP(I,1) POINT(NPT,2)=X(I,1) POINT(NPT,3)=Y(I,1) 10 CONTINUE C C REORGANISE STACKING OF PHOTO POINTS OF PHOTO 1 105 CONTINUE C CONTROL DO 35 I=1,NCON NP(I,1)=CONT(I,1) X(I,1)=CONT(I,2) Y(I,1)=CONT(I,3) 35 CONTINUE C C CHECK N=0 NN=NCON+1 MM=NCON+NCHK IF(NCHK.EQ.0) GO TO 120 DO 40 I=NN,MM N=N+1 NP(I,1)=CHEK(N,1) X(I,1)=CHEK(N,2) Y(I,1)=CHEK(N,3) 40 CONTINUE C C UNKNOWN 120 CONTINUE NNP=MM+NPT IF(NPT.EQ.0) GO TO 125 N=MM+1 NN=0 DO 45 I=N,NNP NN=NN+1 NP(I,1)=POINT(NN,1) X(I,1)=POINT(NN,2) Y(I,1)=POINT(NN,3) 45 CONTINUE 125 CONTINUE C C FIND THE POSITION OF EACH PHOTO POINT FROM PHOTO 1 IN THE PHOTO C POINT STACKS OF THE OTHER PHOTOS DO 50 I=1,NNP ICOUNT=0 DO 55 J=2,NPHOT N=NPC(J) DO 60 K=1,N IF(NP(I,1).NE.NP(K,J)) GO TO 60 P(J)=K ICOUNT=ICOUNT+1 GO TO 55 60 CONTINUE 55 CONTINUE C C REORGANISE STACKING OF PHOTO POINTS IN OTHER PHOTOS TO MATCH C STACKING IN PHOTO 1 DO 65 J=2,NPHOT Q=P(J) IF(Q.EQ.I) GO TO 65 NTEMP=NP(Q,J) XTEMP=X(Q,J) YTEMP=Y(Q,J) NP(Q,J)=NP(I,J) X(Q,J)=X(I,J) Y(Q,J)=Y(I,J) NP(I,J)=NTEMP X(I,J)=XTEMP Y(I,J)=YTEMP 65 CONTINUE 50 CONTINUE RETURN END C C SUBROUTINE OBJECT(IPM4,IPM3,IPM2,IPM1,IP) C ********** ****************************** C ****************************************************************** C * THIS SUBROUTINE SOLVES FOR THE UNKNOWN OBJECT SPACE COORDINATE * C * OF PHOTO POINTS APPEARING ON ALL PHOTOGRAPHS * C ****************************************************************** C IMPLICIT REAL*8 (A-H,O-Z) DIMENSION Q(10),T(20,20),XX(4),YY(4),R(20),F(20),AX(4),AY(4),A(4), $XCC(3),SE(4),SSE(4),SEC(4),SECH(4),SEP(4) COMMON/DATA/ X(150,4),Y(150,4),E(60,3),NUM(60),SP(4),NC(4),NNP, $NPHOT,NKNOWN COMMON/TRANS/ W(20,20),S(60,3),D(4,20),XP(4),YP(4),IPHOT(4) COMMON/NUMBER/ NP(150,4),NPC(4),NCONT,NCON,NCHK,NPT C C INITIALISE COUNTERS FOR ROOT MEAN SQUARE ERROR CALCULATIONS DO 5 I=1,4 SEC(I)=0.0 SECH(I)=0.0 SEP(I)=0.0 SSE(I)=0.0 5 CONTINUE C C CALCULATE DEGREES OF FREEDOM OF SOLUTION DF=2*NPHOT-3 MM=NCON+NCHK JJJ=0 C C FOR EACH PHOTO POINT APPEARING IN THE COMMON OVERLAP CORRECT FOR C LENS DISTORTION AND SOLVE FOR OBJECT SPACE COORDINATES WRITE(6,900) WRITE(6,901) C DO 10 K=1,NNP DO 15 L=1,NPHOT AX(L)=1.0 AY(L)=1.0 IF(IP.EQ.11) GO TO 100 C C REDUCE COMPARATOR COORDINATES TO PRINCIPAL POINT AS ORIGIN XXX=X(K,L)-XP(L) YYY=Y(K,L)-YP(L) C C CALCULATE COEFFICIENTS OF LENS DISTORTION R1=XXX**2 R3=YYY**2 R2=R1+R3 IF(IP.EQ.12) GO TO 105 R4=R2*R2 R6=R2*R4 IF(IP.EQ.14) GO TO 110 DK=D(L,IPM4)*R2+D(L,IPM3)*R4+D(L,IPM2)*R6 R7=2.0*R1+R2 R8=2.0*R3+R2 R9=2.0*XXX*YYY C C APPLY LENS DISTORTION CORRECTIONS XX(L)=X(K,L)-(XXX*DK+D(L,IPM1)*R7+D(L,IP)*R9) YY(L)=Y(K,L)-(YYY*DK+D(L,IPM1)*R9+D(L,IP)*R8) GO TO 15 110 CONTINUE DK=D(L,IPM2)*R2+D(L,IPM1)*R4+D(L,IP)*R6 GO TO 115 105 CONTINUE DK=D(L,IP)*R2 115 CONTINUE XX(L)=X(K,L)-XXX*DK YY(L)=Y(K,L)-YYY*DK GO TO 15 100 CONTINUE XX(L)=X(K,L) YY(L)=Y(K,L) 15 CONTINUE C C TWO ITERATIONS.FIRST WITH UNIT WEIGHT AND UNIT VALUE FOR DIVISOR C 'A'.SECOND WITH REVISED VALUES OF WEIGHT AND 'A' FROM VALUES OF C OBJECT SPACE COORDINATES OBTAINED FROM FIRST ITERATION C DO 20 M=1,2 IF(M.EQ.1) GO TO 120 C DO 25 L=1,NPHOT XXX=X(K,L)-XP(L) YYY=Y(K,L)-YP(L) A(L)=1.0 C COMPUTE REVISED VALUE OF 'A' DO 30 I=1,3 A(L)=A(L)+D(L,I+8)*XCC(I) 30 CONTINUE C C COMPUTE WEIGHTS ASSOCIATED WITH CONDITION EQUATIONS C WEIGHT FOR X AND Y CONDITION EQUATIONS DO 35 II=1,2 GG=0.0 DO 40 I=1,11 R(I)=0.0 F(I)=0.0 40 CONTINUE C IF(II.EQ.2) GO TO 125 C WEIGHT FOR X DO 45 I=1,3 J=I+8 R(I)=XCC(I) R(J)=XCC(I)*X(K,L) 45 CONTINUE R(4)=10.0 GO TO 130 C C WEIGHT FOR Y 125 CONTINUE DO 50 I=5,7 J=I+4 JJ=I-4 R(I)=XCC(JJ) R(J)=XCC(JJ)*Y(K,L) 50 CONTINUE R(8)=10.0 C 130 CONTINUE AB=(A(L)*SP(L))**2 C DO 55 I=1,11 DO 60 J=1,11 F(I)=F(I)+R(J)*W(I,J) 60 CONTINUE GG=GG+F(I)*R(I) 55 CONTINUE C IF(II.EQ.2) GO TO 135 AX(L)=GG+AB AX(L)=DSQRT(AX(L)) GO TO 35 135 CONTINUE AY(L)=GG+AB AY(L)=DSQRT(AY(L)) 35 CONTINUE 25 CONTINUE C 120 CONTINUE C C INITIALISE NORMAL EQUATIONS AND SUM OF RESIDUALS SQUARED DO 65 I=1,3 DO 65 J=1,4 T(I,J)=0.0 65 CONTINUE VV=0.0 C C SET UP CONDITION EQUATIONS FOR EACH PHOTOGRAPH DO 70 L=1,NPHOT C SET UP X AND Y CONDITION EQUATIONS DO 75 II=1,2 GO TO (140,145),II C C SET UP X CONDITION EQUATIONS 140 CONTINUE DO 80 I=1,3 J=I+8 Q(I)=(D(L,J)*XX(L)-D(L,I))/AX(L) 80 CONTINUE Q(4)=(D(L,4)*10.000-XX(L))/AX(L) GO TO 150 C C SET UP Y CONDITION EQUATIONS 145 CONTINUE DO 85 I=1,3 J=I+8 JJ=I+4 Q(I)=(D(L,J)*YY(L)-D(L,JJ))/AY(L) 85 CONTINUE Q(4)=(D(L,8)*10.000-YY(L))/AY(L) 150 CONTINUE C C COMPUTE CONTRIBUTION OF CONDITION EQUATIONS TO SUM OF RESIDUALS C SQUARED AND OBTAIN CUMULATIVE SUM VV=VV+Q(4)**2 C C FORM NORMAL EQUATIONS DO 90 I=1,3 DO 90 J=1,4 T(I,J)=T(I,J)+Q(I)*Q(J) 90 CONTINUE 75 CONTINUE 70 CONTINUE DO 95 I=1,3 Q(I)=T(I,4) 95 CONTINUE C C TEST FOR SINGULARITY OF NORMAL EQUATIONS TOL=1.D-6 C SOLVE NORMAL EQUATIONS CALL SWPMAT(T,1,3,4,KERR,TOL) C C OBTAIN COMPUTED OBJECT SPACE COORDINATES OF POINT AND COMPUTE C CONTRIBUTION OF NORMAL EQUATIONS TO SUM OF RESIDUALS SQUARED, C OBTAIN TOTAL SUM DO 200 I=1,3 XCC(I)=T(I,4) VV=VV-Q(I)*T(I,4) 200 CONTINUE IF(M.EQ.1) GO TO 20 VV=DABS(VV/DF) C C COMPUTE STANDARD ERROR OF UNIT WEIGHT SEUW=DSQRT(VV) SE(4)=0.0 C C COMPUTE VARIANCE/COVARIANCE MATRIX OF COMPUTED COORDINATES DO 205 I=1,3 DO 210 J=1,3 T(I,J)=VV*(T(I,J)) 210 CONTINUE SE(I)=T(I,I) SE(4)=SE(4)+SE(I) 205 CONTINUE 20 CONTINUE C C COMPUTE SUMS OF VARIANCES AND STANDARD ERROR OF COORDINATES JJJ=JJJ+1 DO 215 I=1,4 SSE(I)=SSE(I)+SE(I) IF(JJJ.EQ.NCON) SEC(I)=SSE(I) IF((JJJ.GT.NCON).AND.(JJJ.LE.MM)) SECH(I)=SECH(I)+SE(I) IF(JJJ.GT.MM) SEP(I)=SEP(I)+SE(I) SE(I)=DSQRT(SE(I)) 215 CONTINUE C CWRITE POINT NUMBER,COMPUTED OBJECT SPACE COORDINATES + STANDARD ERRORS C CONTROL,CHECK,UNKNOWN POINTS IF(NCHK.EQ.0) GO TO 155 IF(JJJ.NE.(NCON+1)) GO TO 155 WRITE(6,902) WRITE(6,901) 155 CONTINUE IF(NPT.EQ.0) GO TO 160 IF(JJJ.NE.(MM+1)) GO TO 160 WRITE(6,903) WRITE(6,901) 160 CONTINUE WRITE(6,904) NP(K,1),(XCC(I),I=1,3),(SE(I),I=1,4) 10 CONTINUE C C CALCULATE AVERAGE STANDARD ERRORS FOR CONTROL POINTS,CHECK POINTS, C UNKNOWN POINTS AND FOR ALL POINTS DO 220 I=1,4 SEC(I)=DSQRT(SEC(I)/NCON) IF(NCHK.NE.0) SECH(I)=DSQRT(SECH(I)/NCHK) IF(NPT.NE.0) SEP(I)=DSQRT(SEP(I)/NPT) SSE(I)=DSQRT(SSE(I)/NNP) 220 CONTINUE C CWRITE AVERAGE STANDARD ERRORS WRITE(6,905) NCON,(SEC(I),I=1,4) IF(NCHK.NE.0) WRITE(6,906) NCHK,(SECH(I),I=1,4) IF(NPT.NE.0) WRITE(6,907) NPT,(SEP(I),I=1,4) WRITE(6,908) NNP,(SSE(I),I=1,4) C 900 FORMAT(//,T23,'--- COMPUTED OBJECT SPACE COORDINATES OF CONTROL PO $INTS ---',/) 901 FORMAT(T5,'(POINT)',10X,'(X)',12X,'(Y)',12X,'(Z)',12X,'(SX)',6X,'( $SY)',6X,'(SZ)',4X,'(SPOS)',/) 902 FORMAT(//,T24,'--- COMPUTED OBJECT SPACE COORDINATES OF CHECK POIN $TS ---',/) 903 FORMAT(//,T20,'--- COMPUTED OBJECT SPACE COORDINATES OF UNKNOWN OB $JECT POINTS ---',/) 904 FORMAT(T5,I7,3F13.5,4X,4F8.5) 905 FORMAT(//,T5,'AVERAGE MEAN SQUARE ERRORS FOR',I3,2X,'CONTROL POINT $S ARE',7X,F6.5,3(3X,F6.5)) 906 FORMAT(/,T5,'AVERAGE MEAN SQUARE ERRORS FOR',I3,2X,'CHECK POINTS $ARE',8X,F6.5,3(3X,F6.5)) 907 FORMAT(/,T5,'AVERAGE MEAN SQUARE ERRORS FOR',I3,2X,'UNKNOWN OBJECT $ POINTS ARE',F6.5,3(3X,F6.5)) 908 FORMAT(/,T5,'AVERAGE MEAN SQUARE ERRORS FOR',I3,2X,'POINTS ARE',15 $X,F7.4,3(3X,F6.5)) C RETURN END C C SUBROUTINE SWPMAT(A,IN,N,M,KERR,TOL) C ********** ************************* C ****************************************************************** C * THIS SUBROUTINE IS A MATRIX INVERSION ROUTINE FOR USE IN LEAST * C * SQUARES PROBLEMS.IT DEVELOPES REGRESSION COEFFICIENTS WITHOUT * C * MATRIX MULTIPLICATION. * C * THE ROUTINE WAS DEVELOPED BY THE DEPT. OF AGRONOMY,UNIVERSITY * C * OF ILLINOIS * C ****************************************************************** C IMPLICIT REAL*8 (A-H,O-Z) DIMENSION A(20,20) C KERR=0 DO 5 K=IN,N IF(DABS(A(K,K))-TOL) 100,100,105 105 X=1.0/A(K,K) DO 10 J=IN,M 10 A(K,J)=A(K,J)*X A(K,K)=X DO 15 I=IN,N IF(I-K) 110,15,110 110 Y=A(I,K) A(I,K)=0.0 DO 20 J=IN,M 20 A(I,J)=A(I,J)-Y*A(K,J) 15 CONTINUE 5 CONTINUE 115 RETURN 100 KERR=K GO TO 115 END