📄 relor.c
字号:
double FormNormals( double *norm, double *rhs, PhoParamType leftphoto,
PhoParamType rightphoto, PointType *points, int numpts )
{
int i, j, pt;
double lpart[3][7], lepsilon[3], rpart[3][7], repsilon[3],
bdot[3][6], bdotdot[3][4], r, s, q, dx, dy, dz, sumres2=0;
/* Zero out normal equations */
for (i=1; i<=5+numpts*3; i++) {
rhs[i] = 0;
for (j=i; j<=5+numpts*3; j++) norm[INDUT(i,j)] = 0;
}
for (pt=0; pt<numpts; pt++) {
/* Compute auxiliary variables for left photo */
AuxVars( points[pt], leftphoto, &dx, &dy, &dz, &r, &s, &q);
/* Compute partials with respect to unknowns for left photo */
PartXLYLZL( lpart, leftphoto, r, s, q );
/* Form epsilon (measured - computed), residuals, accumulate residuals-squared */
sumres2 += EpsRes( lepsilon, points[pt].xleft, points[pt].yleft,
&(points[pt].xleft_res), &(points[pt].yleft_res), leftphoto.f, r, s, q );
/* Compute auxiliary variables for right photo */
AuxVars( points[pt], rightphoto, &dx, &dy, &dz, &r, &s, &q);
/* Compute partials with respect to unknowns for right photo */
PartXLYLZL( rpart, rightphoto, r, s, q );
rpart[1][1] = (rightphoto.f/(q*q)) *
(r*(-rightphoto.m33*dy + rightphoto.m32*dz) -
q*(-rightphoto.m13*dy + rightphoto.m12*dz));
rpart[1][2] = (rightphoto.f/(q*q)) *
(r*(rightphoto.cp*dx + rightphoto.so*rightphoto.sp*dy -
rightphoto.co*rightphoto.sp*dz) -
q*(-rightphoto.sp*rightphoto.ck*dx +
rightphoto.so*rightphoto.cp*rightphoto.ck*dy -
rightphoto.co*rightphoto.cp*rightphoto.ck*dz));
rpart[1][3] = -rightphoto.f * s / q;
rpart[2][1] = (rightphoto.f/(q*q)) *
(s*(-rightphoto.m33*dy + rightphoto.m32*dz) -
q*(-rightphoto.m23*dy + rightphoto.m22*dz));
rpart[2][2] = (rightphoto.f/(q*q)) *
(s*(rightphoto.cp*dx + rightphoto.so*rightphoto.sp*dy -
rightphoto.co*rightphoto.sp*dz) -
q*(rightphoto.sp*rightphoto.sk*dx -
rightphoto.so*rightphoto.cp*rightphoto.sk*dy +
rightphoto.co*rightphoto.cp*rightphoto.sk*dz));
rpart[2][3] = rightphoto.f * r/ q;
/* Form epsilon (measured - computed), residuals, accumulate residuals-squared */
sumres2 += EpsRes( repsilon, points[pt].xright, points[pt].yright,
&(points[pt].xright_res), &(points[pt].yright_res),
rightphoto.f, r, s, q );
/* Add contribution of this point to normal equations */
/* Left photo first */
for (i=1; i<=2; i++)
for (j=1; j<=3; j++) bdotdot[i][j] = -lpart[i][j+3];
for (i=1; i<=3; i++) {
for (j=i; j<=3; j++)
norm[INDUT(5+pt*3+i, 5+pt*3+j)] += bdotdot[1][i]*bdotdot[1][j] +
bdotdot[2][i]*bdotdot[2][j];
rhs[5+pt*3+i] += bdotdot[1][i]*lepsilon[1] + bdotdot[2][i]*lepsilon[2];
}
/* Right photo second */
for (i=1; i<=2; i++) {
for (j=1; j<=3; j++) bdot[i][j] = rpart[i][j];
for (j=4; j<=5; j++) bdot[i][j] = rpart[i][j+1];
for (j=1; j<=3; j++) bdotdot[i][j] = -rpart[i][j+3];
}
for (i=1; i<=5; i++) {
for (j=i; j<=5; j++)
norm[INDUT(i,j)] += bdot[1][i]*bdot[1][j] + bdot[2][i]*bdot[2][j];
rhs[i] += bdot[1][i]*repsilon[1] + bdot[2][i]*repsilon[2];
for (j=1; j<=3; j++)
norm[INDUT(i,5+pt*3+j)] += bdot[1][i]*bdotdot[1][j] +
bdot[2][i]*bdotdot[2][j];
}
for (i=1; i<=3; i++) {
for (j=i; j<=3; j++)
norm[INDUT(5+pt*3+i, 5+pt*3+j)] += bdotdot[1][i]*bdotdot[1][j] +
bdotdot[2][i]*bdotdot[2][j];
rhs[5+pt*3+i] += bdotdot[1][i]*repsilon[1] + bdotdot[2][i]*repsilon[2];
}
}
/* Compute and return standard error of unit weight */
if (numpts>5) return sqrt(sumres2/(numpts-5));
else return 1.0e30; /* Zero degrees of freedom, s0 = infinity */
}
void AuxVars( PointType point, PhoParamType photo, double *Pdx, double *Pdy,
double *Pdz, double *Pr, double *Ps, double *Pq )
{
*Pdx = point.X - photo.xl;
*Pdy = point.Y - photo.yl;
*Pdz = point.Z - photo.zl;
*Pr = photo.m11*(*Pdx) + photo.m12*(*Pdy) + photo.m13*(*Pdz);
*Ps = photo.m21*(*Pdx) + photo.m22*(*Pdy) + photo.m23*(*Pdz);
*Pq = photo.m31*(*Pdx) + photo.m32*(*Pdy) + photo.m33*(*Pdz);
}
void PartXLYLZL( double part[3][7], PhoParamType photo, double r, double s, double q )
{
part[1][4] = -photo.f * (r*photo.m31 - q*photo.m11) / (q*q);
part[1][5] = -photo.f * (r*photo.m32 - q*photo.m12) / (q*q);
part[1][6] = -photo.f * (r*photo.m33 - q*photo.m13) / (q*q);
part[2][4] = -photo.f * (s*photo.m31 - q*photo.m21) / (q*q);
part[2][5] = -photo.f * (s*photo.m32 - q*photo.m22) / (q*q);
part[2][6] = -photo.f * (s*photo.m33 - q*photo.m23) / (q*q);
}
double EpsRes( double *epsilon, double x, double y, double *Pxres, double *Pyres,
double f, double r, double s, double q)
{
/* Form epsilon (measured - computed) */
epsilon[1] = x + f * r / q;
epsilon[2] = y + f * s / q;
/* Save residuals */
*Pxres = -epsilon[1];
*Pyres = -epsilon[2];
/* Return sum of residuals-squared */
return (epsilon[1]*epsilon[1] + epsilon[2]*epsilon[2]);
}
void AddCorrections( PhoParamType *Prightphoto, PointType *points, double *rhs,
int numpts, int iter, double s0, FILE *itfile )
{
int pt;
fprintf(itfile, "ITERATION: %d S0 estimate: %6.5lf\n\n\n", iter, s0);
Prightphoto->omega += rhs[1];
Prightphoto->phi += rhs[2];
Prightphoto->kappa += rhs[3];
Prightphoto->yl += rhs[4];
Prightphoto->zl += rhs[5];
fprintf(itfile, "Right photo exterior orientation parameters:\n\n"
"%5s %10s %10s\n", "Param", "Correction", "New Approx");
fprintf(itfile, "%5s %10.5lf %10.5lf (degrees)\n", "omega",
rhs[1] * 180 / M_PI, Prightphoto->omega * 180 / M_PI );
fprintf(itfile, "%5s %10.5lf %10.5lf (degrees)\n", " phi ",
rhs[2] * 180 / M_PI, Prightphoto->phi * 180 / M_PI );
fprintf(itfile, "%5s %10.5lf %10.5lf (degrees)\n", "kappa",
rhs[3] * 180 / M_PI, Prightphoto->kappa * 180 / M_PI );
fprintf(itfile, "%5s %10s %10.5lf\n", "XL ", " ", Prightphoto->xl);
fprintf(itfile, "%5s %10.5lf %10.5lf\n", "YL ", rhs[4], Prightphoto->yl);
fprintf(itfile, "%5s %10.5lf %10.5lf\n", "ZL ", rhs[5], Prightphoto->zl);
fprintf(itfile, "\n\nObject space coordinates:\n\n"
"%8s %10s %10s %10s %10s %10s %10s\n", "Point", "delta X ", "delta Y ",
"delta Z ", "X new ", "Y new ", "Z new ");
for (pt=0; pt<numpts; pt++) {
points[pt].X += rhs[5+pt*3+1];
points[pt].Y += rhs[5+pt*3+2];
points[pt].Z += rhs[5+pt*3+3];
fprintf(itfile, "%8s %10.5lf %10.5lf %10.5lf %10.5lf %10.5lf %10.5lf\n",
points[pt].name, rhs[5+pt*3+1], rhs[5+pt*3+2], rhs[5+pt*3+3],
points[pt].X, points[pt].Y, points[pt].Z );
}
fprintf(itfile, "\n\n\n\n");
}
void OutputResults( char *rootname, PhoParamType leftphoto, PhoParamType rightphoto,
PointType *points, double *norm, double s0, int numpts )
{
FILE *outfile;
char filename[50];
int i;
double lsumx2=0, lsumy2=0, rsumx2=0, rsumy2=0;
strcpy(filename, rootname);
strcat(filename, ".out");
outfile = fopen(filename, "w");
printf("\nResults are in file %s\n", filename);
fprintf(outfile, "Exterior orientation parameters:\n\n");
fprintf(outfile, "%-10s %10s %10s %8s\n", "Parameter", "Left pho",
"Right pho", "SD right");
fprintf(outfile, "%-10s %10.4lf %10.4lf %8.4lf\n", "Omega(deg)",
leftphoto.omega*180/M_PI, rightphoto.omega*180/M_PI,
s0*sqrt(norm[INDUT(1,1)])*180/M_PI);
fprintf(outfile, "%-10s %10.4lf %10.4lf %8.4lf\n", "Phi(deg)",
leftphoto.phi*180/M_PI, rightphoto.phi*180/M_PI,
s0*sqrt(norm[INDUT(2,2)])*180/M_PI );
fprintf(outfile, "%-10s %10.4lf %10.4lf %8.4lf\n", "Kappa(deg)",
leftphoto.kappa*180/M_PI, rightphoto.kappa*180/M_PI,
s0*sqrt(norm[INDUT(3,3)])*180/M_PI );
fprintf(outfile, "%-10s %10.4lf %10.4lf\n", "XL", leftphoto.xl, rightphoto.xl);
fprintf(outfile, "%-10s %10.4lf %10.4lf %8.4lf\n", "YL",
leftphoto.yl, rightphoto.yl, s0*sqrt(norm[INDUT(4,4)]) );
fprintf(outfile, "%-10s %10.4lf %10.4lf %8.4lf\n", "ZL",
leftphoto.zl, rightphoto.zl, s0*sqrt(norm[INDUT(5,5)]) );
fprintf(outfile, "\n\n\nObject space coordinates:\n\n%8s %9s %9s %9s %7s %7s %7s\n",
"point", "X ", "Y ", "Z ", "sdX ", "sdY ", "sdZ ");
for (i=0; i<numpts; i++) {
fprintf(outfile, "%8s %9.4lf %9.4lf %9.4lf %7.4lf %7.4lf %7.4lf\n",
points[i].name, points[i].X, points[i].Y, points[i].Z,
s0*sqrt(norm[INDUT(5+i*3+1,5+i*3+1)]),
s0*sqrt(norm[INDUT(5+i*3+2,5+i*3+2)]),
s0*sqrt(norm[INDUT(5+i*3+3,5+i*3+3)]) );
}
fprintf(outfile, "\n\n\nPhoto coordinate residuals:\n\n"
"%8s %7s %7s %7s %7s\n", "point", "xl-res", "yl-res", "xr-res", "yr-res");
for (i=0; i<numpts; i++) {
fprintf(outfile, "%8s %7.4lf %7.4lf %7.4lf %7.4lf\n",
points[i].name, points[i].xleft_res, points[i].yleft_res,
points[i].xright_res, points[i].yright_res );
lsumx2 += points[i].xleft_res * points[i].xleft_res;
lsumy2 += points[i].yleft_res * points[i].yleft_res;
rsumx2 += points[i].xright_res * points[i].xright_res;
rsumy2 += points[i].yright_res * points[i].yright_res;
}
fprintf(outfile, "\n%8s %7.4lf %7.4lf %7.4lf %7.4lf\n",
"RMS", sqrt(lsumx2/numpts), sqrt(lsumy2/numpts),
sqrt(lsumx2/numpts), sqrt(lsumy2/numpts) );
fprintf(outfile, "\n\n\nStandard error of unit weight: %7.4lf\n"
"Degrees of freedom: %d\n", s0, numpts-5);
fclose(outfile);
}
void pause(void)
{
printf("Press a key to end program.");
getch();
}
/*
This program performs a relative orientation solution for a stereopair of
near vertical photos by least squares. No weights are used for photo
coordinates or object space coordinates. All x and y photo coordinates
must be corrected for principal point offsets, lens distortions, and
atmospheric refraction as needed. Data file must be plain ASCII text and
have a .dat extension. The format of the file is as follows:
Line 1: <focal length>
Lines 2 through end of file:
<point name> <left photo x> <left photo y> <right photo x> <right photo y>
Sample data file:
152.113
a -4.878 1.974 -97.920 -2.923
b 89.307 2.709 -1.507 -1.856
c 0.261 84.144 -90.917 78.970
d 90.334 83.843 -1.571 79.470
e -4.668 -86.821 -100.060 -95.748
f 88.599 -85.274 -0.965 -94.319
Output is sent to a file with the same root name and a .out extension.
Iteration information is sent to a file with a .itr extension.
*/
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -