?? 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;
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -