单片空间后方交会C程序
#include "stdio.h"
#include "stdlib.h"
#include "math.h"
#include
#define f 153.840e-3
#define x0 0
#define y0 0
#define m0 2500
#define pi 3.1415926
void MatrixReverse(double* nm, double* m, int r, int c)
{//求转置矩阵
int i,j;
for(i=0;i
for(j=0;j
*(nm+j*r+i)=*(m+i*c+j);
}
}
}
bool RowMul(double* m, int r, int c, double ratio)
{//求矩阵的倍数
int i;
for(i=0;i
*(m+r*c+i)*=ratio;
}
return true;
}
bool RowSwap(double* m, int r1, int r2, int c)
{//矩阵行交换
int i;
double temp;
for(i=0;i
temp=*(m+r1*c+i);
*(m+r1*c+i)=*(m+r2*c+i);
*(m+r2*c+i)=temp;
}
return true;
}
bool RowMinus(double* m, int r1, int r2, int c)
{//矩阵行相减
int i;
for(i=0;i
*(m+r1*c+i)-=*(m+r2*c+i);
}
return true;
}
void MatrixPlus(double* m1, double* m2, int r, int c)
{//矩阵相加
int i,j;
for(i=0;i
*(m1+i*c+j)+=*(m2+i*c+j);
}
}
void MatrixMul(double* nm, double* m1, double* m2, int r, int c,int cr)
{//求两个矩阵的积
int i,j,k;
for(i=0;i
*(nm+i*c+j)=0;
for(k=0;k
*(nm+i*c+j)+=*(m1+i*cr+k)**(m2+k*c+j);
}
}
}
void MatrixInverse(double* nm, double* m, int rc)
{//求矩阵的逆
double* nme=new double[2*rc*rc];
int i,j;
for(i=0;i
for(j=0;j<2*rc;j++)
{
if(i==(j-rc))*(nme+i*2*rc+j)=1;
else if(j
}
}
for(i=0;i
if(*(nme+i*2*rc+i)==0)
{
for(j=i;j
if(*(nme+j*2*rc+i))
{
RowSwap(nme, i, j, 2*rc);
break;
}
}
}
double ratio;
for(j=i+1;j
if(*(nme+j*2*rc+i))
{
ratio=*(nme+i*2*rc+i)/(*(nme+j*2*rc+i));
RowMul(nme, j, 2*rc, ratio);
RowMinus(nme, j, i, 2*rc);
}
}
}
for(i=0;i
double ratio;
for(j=i+1;j
if(*(nme+(rc-1-j)*2*rc+(rc-1-i)))
{
ratio=*(nme+(rc-1-i)*2*rc+(rc-1-i))/(*(nme+(rc-1-j)*2*rc+(rc-1-i)));
RowMul(nme, (rc-1-j), 2*rc, ratio);
RowMinus(nme, (rc-1-j), (rc-1-i), 2*rc);
}
}
}
for(i=0;i
double ratio;
ratio=1/(*(nme+i*2*rc+i));
RowMul(nme, i, 2*rc, ratio);
}
for(i=0;i
delete []nme;
}
void main()
{
int counter = 0;
FILE * fp = fopen("考试所用数据.DAT","r");
fscanf(fp,"%d",&counter);
double xx[100],yy[100],XX[100],YY[100],ZZ[100],PID[100];
for (int c=0;c
fscanf(fp,"%lf%lf%lf%lf%lf%lf",&PID[c],&xx[c],&yy[c],&YY[c],&XX[c],&ZZ[c]);
xx[c] /= 1000;
yy[c] /= 1000;
}
fclose(fp);
printf("像片的内方位元数:x0=y0=0；航摄仪焦距:f=153.84mm。\n摄影比例尺:1/m=1/2500\n\n");
double phi,omega,kappa,XS,YS,ZS;
double dphi,domega,dkappa;
phi=o
mega=kappa=XS=YS=ZS=0;
int j;
for(j=0;j
XS+=XX[j];
YS+=YY[j];
}
XS/=counter;//初始值
YS/=counter;
ZS+=f*m0;
dphi=domega=dkappa=1;
int count=0;
while((dphi>0.00003 || domega>0.00003 || dkappa>0.00003))
{//迭代验证
count++;
int i;
double ATA[36]={0};
double ATL[6]={0};
double a1=cos(phi)*cos(kappa)-sin(phi)*sin(omega)*sin(kappa);
double a2=-cos(phi)*sin(kappa)-sin(phi)*sin(omega)*cos(kappa);
double a3=-sin(phi)*cos(omega);
double b1=cos(omega)*sin(kappa);
double b2=cos(omega)*cos(kappa);
double b3=-sin(omega);
double c1=sin(phi)*cos(kappa)+cos(phi)*sin(omega)*sin(kappa);
double c2=-sin(phi)*sin(kappa)+cos(phi)*sin(omega)*cos(kappa);
double c3=cos(phi)*cos(omega);
for(i=0;i
double X,Y,Z,x,y;
double fX,fY,fZ;
double id;
id=PID[i];
x=xx[i];
y=yy[i];
X=XX[i];
Y=YY[i];
Z=ZZ[i];
fX=a1*(X-XS)+b1*(Y-YS)+c1*(Z-ZS);
fY=a2*(X-XS)+b2*(Y-YS)+c2*(Z-ZS);
fZ=a3*(X-XS)+b3*(Y-YS)+c3*(Z-ZS);
double A[12], AT[12], L[2], temp1[36], temp2[6];
L[0]=x+f*fX/fZ;
L[1]=y+f*fY/fZ;
A[0]=(a1*f+a3*(x-x0))/(fZ);
A[1]=(b1*f+b3*(x-x0))/(fZ);
A[2]=(c1*f+c3*(x-x0))/(fZ);
A[3]=(y-y0)*sin(omega)-((x-x0)*((x-x0)*cos(kappa)-(y-y0)*sin(kappa))/f+f*cos(kappa))*cos(omega);
A[4]=-f*sin(kappa)-(x-x0)*((x-x0)*sin(kappa)+(y-y0)*cos(kappa))/f;
A[5]=y-y0;
A[6]=(a2*f+a3*(y-y0))/(fZ);
A[7]=(b2*f+b3*(y-y0))/(fZ);
A[8]=(c2*f+c3*(y-y0))/(fZ);
A[9]=-(x-x0)*sin(omega)-((y-y0)*((x-x0)*cos(kappa)-(y-y0)*sin(kappa))/f-f*sin(kappa))*cos(omega);
A[10]=-f*cos(kappa)-(y-y0)*((x-x0)*sin(kappa)+(y-y0)*cos(kappa))/f;
A[11]=x0-x;
MatrixReverse(AT,A,2,6);
MatrixMul(temp1,AT,A,6,6,2);//求一个点的ATA
MatrixMul(temp2,AT,L,6,1,2);//求一个点的ATL
MatrixPlus(ATA,temp1,6,6);//系数矩阵相加
MatrixPlus(ATL,temp2,6,1);
}
//解法方程
double temp[6],temp0[36];
MatrixInverse(temp0,ATA,6);
MatrixMul(temp,temp0,ATL,6,1,6);
XS+=temp[0];
YS+=temp[1];
ZS+=temp[2];
phi+=temp[3];
omega+=temp[4];
kappa+=temp[5];
if(temp[3]<0)
dphi=temp[3]*(-1);
else dphi=temp[3];
if(temp[4]<0)
domega=temp[4]*(-1);
else domega=temp[4];
if(temp[5]<0)
dkappa=temp[5]*(-1);
else dkappa=temp[5];
//精度评定
double MM[6],VTV[1],M0;
for (i=0;i<6;i++)
{
MM[i]=sqrt(temp0[i*6+i]);
}
VTV[0]=0;
for(i=0; i
double X,Y,Z,x,y;
double fX,fY,fZ;
double tempV[2],tempVT[2],tempVTV[1];
x=xx[i];
y=yy[i];
X=XX[i];
Y=YY[i];
Z=ZZ[i];
fX=a1*(X-XS)+b1*(Y-YS)+c1*(Z-ZS);
fY=a2*(X-XS)+b2*(Y-YS)+c2*(Z-ZS);
fZ=a3*(X-XS)+b3*(Y-YS)+c3*(Z-ZS);
tempV[0]=x+f*fX/fZ;
tempV[1]=y+f*fY/fZ;
MatrixReverse(tempVT,tempV,2,1);
MatrixMul(tempVTV,tempVT,tempV,1,1,2);
MatrixPlus(VTV,tempVTV,1,1);
}
M0 = sqrt(VTV[0]/(counter*2-6));
printf("输入坐标点个数：%d\n\n",counter);
printf("计算结果：\n(1)迭代次数：%d\n",count);
printf("(2)外方位元素计算值: \n phi=%lf omega=%lf kappa=%lf\n Xs=%lf Ys=%lf Zs=%lf\n",(phi*180/pi),(omega*180/pi),(kappa*180/pi),XS,YS,ZS);
printf("(3)单位权中误差：%lf\n",M0);
printf("(4)精度：\n");
for(i=3;i<6;i++)
{
MM[i]*=M0;
printf(" %lf \n",MM[i]);
}
for(i=0;i<3;i++)
{
MM[i]*=M0;
printf(" %lf \n",MM[i]);
}
printf("\n\n");
}}