3dconf.c

摄影测量中的三维空间变换程序,供摄影测量初学者使用

#include	<stdio.h>
#include	<stdlib.h>
#include	<string.h>
#include	<math.h>
#include	<conio.h>




/* Maximum numbers of common and unknown points. Change as needed */
#define		MAXCOM		200
#define		MAXUNK		400

#define     M_PI        3.141592653589793238

/* Macro to access upper triangular matrix stored as 1D array */
#define     INDUT(i,j)  ( (((j) * ((j)-1)) / 2) + i )

/* Macro to compute the square of a number */
#define		SQR(x)		((x)*(x))




typedef struct {
	char	name[16];
	double	xcon, ycon, zcon, xarb, yarb, zarb, xres, yres, zres;
} CommonPointType;

typedef struct {
	char	name[16];
	double	x, y, z;
} UnkPointType;

typedef struct {
    double  m11, m12, m13,
            m21, m22, m23,
            m31, m32, m33,
            so, sp, sk,
            co, cp, ck,
			st, ss, sa,
			ct, cs, ca;
} RotationMatrixType;




void ReadData( char *rootname, CommonPointType *ComPoints, UnkPointType *UnkPoints, 
			  int *Pnumcom, int *Pnumunk );
void InitApprox( CommonPointType *ComPoints, int numcom, double *params, FILE *outf);
void solve( double *a, double *b, int n, int invflag );
void RotationMatrixOPK(double omega, double phi, double kappa,
					   RotationMatrixType *PRotMat);
void RotationMatrixTSA(double tilt, double swing, double azimuth,
					   RotationMatrixType *PRotMat);
double FormNormals(double *norm, double *rhs, RotationMatrixType RotMat, 
				   CommonPointType *ComPoints, int numcom, double *params);
void FormAI( double AI[4][8], RotationMatrixType RotMat, double scale,
			double x, double y, double z );
void AddCorrections( double *rhs, double *params );
void PrintIter( double *rhs, double *params, CommonPointType *ComPoints, int numcom,
			   double s0, int iter, FILE *outf, char pnames[8][8] );
void PrintResults( FILE *outf, double *norm, double s0, double *params, int numunk,
				  UnkPointType *UnkPoints, char pnames[8][8] );
void pause(void);




void main(void)
{
	char				rootname[50], filename[50];
	int					numcom, numunk, iter=0, converge=0, diverge=0;
	double				s0=1.0e30, s0old, norm[29], rhs[8], params[8];
	CommonPointType		ComPoints[MAXCOM];
	UnkPointType		UnkPoints[MAXUNK];
	RotationMatrixType	RotMat;
	FILE				*outf;
	char				pnames[8][8] = {"", "scale", "omega", "phi", "kappa", 
										"Tx", "Ty", "Tz"};

	ReadData( rootname, ComPoints, UnkPoints, &numcom, &numunk );

	/* Open output file (use .out extension) */
	strcpy(filename, rootname);
	strcat(filename, ".out");
	outf = fopen(filename, "w");

	/* Compute initial approximations for unknown parameters */
	InitApprox( ComPoints, numcom, params, outf );

	do {
		iter++;         /* Increment iteration count */
		s0old = s0;     /* Save former value of s0 for convergence test */
		/* Compute rotation matrix elements */
		RotationMatrixOPK( params[2], params[3], params[4], &RotMat); 
		s0 = FormNormals(norm, rhs, RotMat, ComPoints, numcom, params);
		printf("ITERATION %d     S0 = %6.5lf\n", iter, s0);

		/* Check for convergence or divergence */
		if (fabs(s0old-s0)/s0 < 0.0001) converge=1;
		else if (s0 > s0old) diverge=1;

		/* Solve for corrections
		   If converged or diverged call for inverse also */
		solve(norm, rhs, 7, converge|diverge);
		AddCorrections( rhs, params );
		PrintIter( rhs, params, ComPoints, numcom, s0, iter, outf, pnames );
	} while (!converge && !diverge);
	
	PrintResults( outf, norm, s0, params, numunk, UnkPoints, pnames );

	fclose(outf);
	pause();
}




void ReadData( char *rootname, CommonPointType *ComPoints, UnkPointType *UnkPoints, 
			  int *Pnumcom, int *Pnumunk )
{
    char    filename[50], tempstr[50];
    FILE    *infile;
    int     done;

	/* Get name of input file and open if it exists */
    printf("Enter root name of data file (.dat assumed) --> ");
    scanf("%s", rootname);
    strcpy(filename, rootname);
    strcat(filename, ".dat");
    printf("\n");
    infile = fopen(filename, "r");
    if (infile == NULL) {
        printf("ERROR. Could not open file %s\n", filename);
		pause();
        exit(1);
    }

	/* Read coordinates of common points until the end flag character, # */
	*Pnumcom = 0;
	done = 0;
	do {
		fscanf(infile, "%s", tempstr);
		if (strcmp(tempstr, "#") == 0) done = 1;
		else {
			if (*Pnumcom == MAXCOM) {
				printf("ERROR. More than %d common points in data file.\n", MAXCOM);
				pause();
				exit(1);
			}
			else {
				strcpy( ComPoints[*Pnumcom].name, tempstr );
				fscanf(infile, "%lf %lf %lf %lf %lf %lf", &(ComPoints[*Pnumcom].xarb),
					&(ComPoints[*Pnumcom].yarb), &(ComPoints[*Pnumcom].zarb),
					&(ComPoints[*Pnumcom].xcon), &(ComPoints[*Pnumcom].ycon), 
					&(ComPoints[*Pnumcom].zcon) );
				(*Pnumcom)++;
			}
		}
	} while (!done);

	/* Be sure there are enough points */
	if (*Pnumcom < 3) {
		printf("ERROR. Less than 3 common points.\n");
		pause();
		exit(1);
	}

	/* Read coordinates of unknown points until the end flag character, # */
	*Pnumunk = 0;
	done = 0;
	do {
		fscanf(infile, "%s", tempstr);
		if (strcmp(tempstr, "#") == 0) done = 1;
		else {
			if (*Pnumunk == MAXUNK) {
				printf("ERROR. More than %d unknown points in data file.\n", MAXUNK);
				pause();
				exit(1);
			}
			else {
				strcpy( UnkPoints[*Pnumunk].name, tempstr );
				fscanf(infile, "%lf %lf %lf", &(UnkPoints[*Pnumunk].x),
					&(UnkPoints[*Pnumunk].y), &(UnkPoints[*Pnumunk].z) );
				(*Pnumunk)++;
			}
		}
	} while (!done);

	fclose(infile);
	
}




void InitApprox( CommonPointType *ComPoints, int numcom, double *params, FILE *outf )
/*	This function computes initial approximations for scale, omega, phi, kappa,
	Tx, Ty, and Tz. The method used is from:
	
	  Dewitt, B.A.: "Initial Approximations for the Three-Dimensional Conformal
			Coordinate Transformation, Photogrammetric Engineering and Remote
			Sensing, vol. 62, no. 1, p. 79, 1996.
*/
{
	int		numscale, i, j, pt1, pt2, pt3, ind1, ind2, ind3;
	double	sumscale, distcon, distarb, maxaltsq, dsq12, dsq23, dsq13, a2, b2, c2, h2,
			a_arb, b_arb, c_arb, a_con, b_con, c_con, arb_tilt, arb_az,
			con_tilt, con_az, x_arb1, y_arb1, x_arb2, y_arb2,
			x_con1, y_con1, x_con2, y_con2, azimuth_con, azimuth_arb, swing,
			txsum, tysum, tzsum;
	RotationMatrixType	ArbRotMat, ConRotMat, FullRotMat;
	
	/* Calculate approximation for scale using all pairs of points */
	numscale = 0;
	sumscale = 0;
	for (i=0; i<numcom-1; i++) {
		for (j=i+1; j<numcom; j++) {
			distcon = sqrt(	SQR(ComPoints[i].xcon - ComPoints[j].xcon) +
							SQR(ComPoints[i].ycon - ComPoints[j].ycon) +
							SQR(ComPoints[i].zcon - ComPoints[j].zcon)   );
			distarb = sqrt(	SQR(ComPoints[i].xarb - ComPoints[j].xarb) +
							SQR(ComPoints[i].yarb - ComPoints[j].yarb) +
							SQR(ComPoints[i].zarb - ComPoints[j].zarb)   );
			sumscale += distcon / distarb;
			numscale++;
		}
	}
	params[1] = sumscale / numscale;

	/*  Find geometrically strongest triangle of 3 points
		Strength is based on triangle with the largest altitude
		Altitude is defined as the perpendicular distance from the longest
		leg (or extension thereof) to the point not on the longest leg */
	maxaltsq = 0;
	for (ind1=0; ind1<numcom-2; ind1++) {
		for (ind2=ind1+1; ind2<numcom-1; ind2++) {
			dsq12 = SQR(ComPoints[ind1].xcon - ComPoints[ind2].xcon) +
					SQR(ComPoints[ind1].ycon - ComPoints[ind2].ycon) +
					SQR(ComPoints[ind1].zcon - ComPoints[ind2].zcon);
			for (ind3=ind2+1; ind3<numcom; ind3++) {
				dsq13 = SQR(ComPoints[ind1].xcon - ComPoints[ind3].xcon) +
						SQR(ComPoints[ind1].ycon - ComPoints[ind3].ycon) +
						SQR(ComPoints[ind1].zcon - ComPoints[ind3].zcon);
				dsq23 = SQR(ComPoints[ind2].xcon - ComPoints[ind3].xcon) +
						SQR(ComPoints[ind2].ycon - ComPoints[ind3].ycon) +
						SQR(ComPoints[ind2].zcon - ComPoints[ind3].zcon);

				if ((dsq12 >= dsq13) && (dsq12 >= dsq23)) {
					c2 = dsq12;
					a2 = dsq13;
					b2 = dsq23;
				}
				else {
					if ((dsq13 >= dsq12) && (dsq13 >= dsq23)) {
						c2 = dsq13;
						a2 = dsq12;
						b2 = dsq23;
					}
					else {
						c2 = dsq23;
						a2 = dsq12;
						b2 = dsq13;
					}
				}
				h2 = (2*(c2*(a2+b2)+a2*b2)-a2*a2-b2*b2-c2*c2)/(4*c2);
				if (h2 < 0) {
					printf("ERROR IN ALTITUDE COMPUTATION\n");
					pause();
					exit(1);
				}
				if (h2 > maxaltsq) {
					pt1 = ind1;
					pt2 = ind2;
					pt3 = ind3;
					maxaltsq = h2;
				}
			}
		}
	}

	/*  Compute coefficients of equation of plane through the 3 points
		in the arbitrary system */
	a_arb = ComPoints[pt1].yarb*ComPoints[pt2].zarb +
			ComPoints[pt2].yarb*ComPoints[pt3].zarb +
			ComPoints[pt3].yarb*ComPoints[pt1].zarb -
			ComPoints[pt1].yarb*ComPoints[pt3].zarb -
			ComPoints[pt2].yarb*ComPoints[pt1].zarb -
			ComPoints[pt3].yarb*ComPoints[pt2].zarb;
	b_arb = -ComPoints[pt1].xarb*ComPoints[pt2].zarb -
			ComPoints[pt2].xarb*ComPoints[pt3].zarb -
			ComPoints[pt3].xarb*ComPoints[pt1].zarb +
			ComPoints[pt1].xarb*ComPoints[pt3].zarb +
			ComPoints[pt2].xarb*ComPoints[pt1].zarb +
			ComPoints[pt3].xarb*ComPoints[pt2].zarb;
	c_arb = ComPoints[pt1].xarb*ComPoints[pt2].yarb +
			ComPoints[pt2].xarb*ComPoints[pt3].yarb +
			ComPoints[pt3].xarb*ComPoints[pt1].yarb -
			ComPoints[pt1].xarb*ComPoints[pt3].yarb -
			ComPoints[pt2].xarb*ComPoints[pt1].yarb -
			ComPoints[pt3].xarb*ComPoints[pt2].yarb;


	/*  Compute coefficients of equation of plane through the 3 points
		in the control system */
	a_con = ComPoints[pt1].ycon*ComPoints[pt2].zcon +
			ComPoints[pt2].ycon*ComPoints[pt3].zcon +
			ComPoints[pt3].ycon*ComPoints[pt1].zcon -
			ComPoints[pt1].ycon*ComPoints[pt3].zcon -
			ComPoints[pt2].ycon*ComPoints[pt1].zcon -
			ComPoints[pt3].ycon*ComPoints[pt2].zcon;
	b_con = -ComPoints[pt1].xcon*ComPoints[pt2].zcon -
			ComPoints[pt2].xcon*ComPoints[pt3].zcon -
			ComPoints[pt3].xcon*ComPoints[pt1].zcon +
			ComPoints[pt1].xcon*ComPoints[pt3].zcon +
			ComPoints[pt2].xcon*ComPoints[pt1].zcon +
			ComPoints[pt3].xcon*ComPoints[pt2].zcon;
	c_con = ComPoints[pt1].xcon*ComPoints[pt2].ycon +
			ComPoints[pt2].xcon*ComPoints[pt3].ycon +
			ComPoints[pt3].xcon*ComPoints[pt1].ycon -
			ComPoints[pt1].xcon*ComPoints[pt3].ycon -
			ComPoints[pt2].xcon*ComPoints[pt1].ycon -
			ComPoints[pt3].xcon*ComPoints[pt2].ycon;

	/* Compute tilt & azimuth of plane in arbitrary system */
	arb_tilt = atan2( c_arb, hypot(a_arb, b_arb) ) + M_PI/2;
	arb_az = atan2( a_arb, b_arb );
	/* Compute tilt & azimuth of plane in control system */
	con_tilt = atan2( c_con, hypot(a_con, b_con) ) + M_PI/2;
	con_az = atan2( a_con, b_con );
	/*  Compute the corresponding rotation matrices with swing = 0 */
	RotationMatrixTSA(arb_tilt, 0.0, arb_az, &ArbRotMat);
	RotationMatrixTSA(con_tilt, 0.0, con_az, &ConRotMat);
	/*  Rotate arbitrary and control coordinates for points 1 and 2
		to get a line in the arbitrary system and the corresponding
		line in the control system.  Don't need to rotate Z because
		with these rotations all of the z values must be the same. */
	x_arb1 =	ArbRotMat.m11*ComPoints[pt1].xarb +
				ArbRotMat.m12*ComPoints[pt1].yarb +
				ArbRotMat.m13*ComPoints[pt1].zarb;
	y_arb1 =	ArbRotMat.m21*ComPoints[pt1].xarb +
				ArbRotMat.m22*ComPoints[pt1].yarb +
				ArbRotMat.m23*ComPoints[pt1].zarb;
	x_arb2 =	ArbRotMat.m11*ComPoints[pt2].xarb +
				ArbRotMat.m12*ComPoints[pt2].yarb +
				ArbRotMat.m13*ComPoints[pt2].zarb;
	y_arb2 =	ArbRotMat.m21*ComPoints[pt2].xarb +
				ArbRotMat.m22*ComPoints[pt2].yarb +
				ArbRotMat.m23*ComPoints[pt2].zarb;
	x_con1 =	ConRotMat.m11*ComPoints[pt1].xcon +
				ConRotMat.m12*ComPoints[pt1].ycon +
				ConRotMat.m13*ComPoints[pt1].zcon;
	y_con1 =	ConRotMat.m21*ComPoints[pt1].xcon +
				ConRotMat.m22*ComPoints[pt1].ycon +
				ConRotMat.m23*ComPoints[pt1].zcon;
	x_con2 =	ConRotMat.m11*ComPoints[pt2].xcon +
				ConRotMat.m12*ComPoints[pt2].ycon +
				ConRotMat.m13*ComPoints[pt2].zcon;
	y_con2 =	ConRotMat.m21*ComPoints[pt2].xcon +
				ConRotMat.m22*ComPoints[pt2].ycon +
				ConRotMat.m23*ComPoints[pt2].zcon;
	/* Get swing by subtracting azimuths */
	azimuth_con = atan2( x_con2-x_con1, y_con2-y_con1 );
	azimuth_arb = atan2( x_arb2-x_arb1, y_arb2-y_arb1 );
	swing = azimuth_con - azimuth_arb;
	RotationMatrixTSA(arb_tilt, swing, arb_az, &ArbRotMat);
	/*  Now compute (ConRotMat:transpose * ArbRotMat):transpose
		This is overall rotation matrix */
	FullRotMat.m11 = ConRotMat.m11*ArbRotMat.m11 + ConRotMat.m21*ArbRotMat.m21 +
					 ConRotMat.m31*ArbRotMat.m31;
	FullRotMat.m21 = ConRotMat.m11*ArbRotMat.m12 + ConRotMat.m21*ArbRotMat.m22 +
					 ConRotMat.m31*ArbRotMat.m32;
	FullRotMat.m31 = ConRotMat.m11*ArbRotMat.m13 + ConRotMat.m21*ArbRotMat.m23 +
					 ConRotMat.m31*ArbRotMat.m33;
	FullRotMat.m12 = ConRotMat.m12*ArbRotMat.m11 + ConRotMat.m22*ArbRotMat.m21 +
					 ConRotMat.m32*ArbRotMat.m31;