1、#include#include#define n 4 /控制点个数#define PI 3.14159265struct coordinatedouble x; /像点坐标double y;double Xt; /控制点坐标double Yt;double Zt;/ void inverse(double c66) /矩阵求逆/ / int i,j,h,k;/ double p;/ double q612;/ for(i=0;i0;k-,h-) / 消去对角线以上的数据/ for(i=k-1;i=0;i-)/ / if(qih=0)/ continue;/ p=qkh/qih;/ / p=q
2、ih/qkh;/ for(j=11;j0;j-)/ / qij*=p;/ qij-=qkj;/ / / for(i=0;i6;i+)/将对角线上数据化为 1/ / p=1.0/qii;/ for(j=0;j12;j+)/ qij*=p;/ / for(i=0;i6;i+) /提取逆矩阵/ for(j=0;jn;j+)/ / cij=qij+6;/ / void ContraryMatrix(double *const pMatrix, double *const _pMatrix, const int int flag=0;double *tMatrix = new double2*dim*d
3、im;for (int i=0; idim; i+)for (int j=0; jdim; j+)tMatrixi*dim*2+j = pMatrixi*dim+j; for (i=0; idim; i+)for (int j=dim; jdim*2; j+)tMatrixi*dim*2+j = 0.0;tMatrixi*dim*2+dim+i = 1.0; /Initialization over!for (i=0; idim; i+)/Process Colsdouble base = tMatrixi*dim*2+i;if (fabs(base) 1E-300)for (ii=i;iid
4、im;ii+)if(tMatrixii*dim*2+i!=0)flag=1;for (jj=0;jj2*dim;jj+)tMatrixi*dim*2+jj+=tMatrixii*dim*2+jj;base = tMatrixi*dim*2+i;if (flag=0)printf( “求逆矩阵过程中被零除，无法求解!“) ;/ exit(0);for (int j=0; jdim; j+)/rowif (j = i) continue;double times = tMatrixj*dim*2+i/base;for (int k=0; kdim*2; k+)/col tMatrixj*dim*2
5、+k = tMatrixj*dim*2+k - times*tMatrixi*dim*2+k; for (int k=0; kdim*2; k+)tMatrixi*dim*2+k /= base;for (i=0; idim; i+)for (int j=0; jdim; j+)_pMatrixi*dim+j = tMatrixi*dim*2+j+dim; delete tMatrix;void main()double f,xo,yo; /内方位元素int m; /摄影比例尺分母f=0.15324;xo=0;yo=0;m=50000;struct coordinate coorn=-0.08
6、615,-0.06899,36589.41,25273.32,2195.17,-0.05340,0.08221,37631.08,31324.51,728.69,-0.01478,-0.07663,39100.97,24934.98,2386.50,0.01046,0.06443,40426.54,30319.81,757.31; int i,j; /循环变量double Xs,Ys,Zs; /外方位线元素double phi,omega,kappa; /外方位角元素double R33; /旋转矩阵 Rdouble (x)n,(y)n; /控制点像点坐标的近似值double A86; /误差
7、方程中的矩阵 Adouble ATA66; /矩阵 A 的转置矩阵与 A 的乘积double L81;double ATL61; /矩阵 A 的转置矩阵与 L 的乘积double _ATA66;double X61; /未知数矩阵double V81; /改正数矩阵double DXs,DYs,DZs,Dphi,Domega,Dkappa; /6 个外方位元素的改正值/与精度计算有关的量double m0; /单位权中误差double Q66; /协方差矩阵double m1,m2,m3,m4,m5,m6; /未知数 Xs,Ys,Zs,phi,omega,kappa 的中误差矩阵Xs=0;Ys
8、=0;for(i=0;in;i+)Xs+=coori.Xt;Ys+=coori.Yt;/外方位元素的初始值Xs/=n; Ys/=n;Zs=f*m;phi=0;omega=0;kappa=0; /在竖直摄影情况下do/计算旋转矩阵 R 的各个元素R00=cos(phi)*cos(kappa)-sin(phi)*sin(omega)*sin(kappa);R01=-cos(phi)*sin(kappa)-sin(phi)*sin(omega)*cos(kappa);R02=-sin(phi)*cos(omega);R10=cos(omega)*sin(kappa);R11=cos(omega)*c
9、os(kappa);R12=-sin(omega);R20=sin(phi)*cos(kappa)+cos(phi)*sin(omega)*sin(kappa);R21=-sin(phi)*sin(kappa) + cos(phi)*sin(omega)*cos(kappa);R22=cos(phi)*cos(omega);/将未知数的近似值代人共线方程式，计算(x),(y)for(i=0;in;i+)(x)i=-f*(R00*(coori.Xt-Xs)+R10*(coori.Yt-Ys)+R20*(coori.Zt-Zs)/(R02*(coori.Xt-Xs)+R12*(coori.Yt-Y
10、s)+R22*(coori.Zt-Zs);(y)i=-f*(R01*(coori.Xt-Xs)+R11*(coori.Yt-Ys)+R21*(coori.Zt-Zs)/(R02*(coori.Xt-Xs)+R12*(coori.Yt-Ys)+R22*(coori.Zt-Zs);/计算矩阵 A 的各个元素for(i=0;in;i+)A2*i0=(R00*f + R02 * (coori.x - xo) / (R02*(coori.Xt - Xs) + R12*(coori.Yt - Ys) + R22*(coori.Zt - Zs);A2*i1=(R10*f + R12 * (coori.x -
11、 xo) / (R02*(coori.Xt - Xs) + R12*(coori.Yt - Ys) + R22*(coori.Zt - Zs);A2*i2=(R20*f + R22 * (coori.x - xo) / (R02*(coori.Xt - Xs) + R12*(coori.Yt - Ys) + R22*(coori.Zt - Zs);A2*i3=(coori.y-yo)*sin(omega)-(coori.x-xo)/f*(coori.x-xo)*cos(kappa)-(coori.y-yo)*sin(kappa)+f*cos(kappa)*cos(omega);A2*i4=-f
12、*sin(kappa)-(coori.x-xo)/f*(coori.x-xo)*sin(kappa)+(coori.y-yo)*cos(kappa);A2*i5=coori.y-yo;A2*i+10=(R01*f + R02 * (coori.y - yo) / (R02*(coori.Xt - Xs) + R12*(coori.Yt - Ys) + R22*(coori.Zt - Zs);A2*i+11=(R11*f + R12 * (coori.y - yo) / (R02*(coori.Xt - Xs) + R12*(coori.Yt - Ys) + R22*(coori.Zt - Zs
13、);A2*i+12=(R21*f + R22 * (coori.y - yo) / (R02*(coori.Xt - Xs) + R12*(coori.Yt - Ys) + R22*(coori.Zt - Zs);A2*i+13=-(coori.x-xo)*sin(omega)-(coori.y-yo)/f*(coori.x-xo)*cos(kappa)-(coori.y-yo)*sin(kappa)-f*sin(kappa)*cos(omega);A2*i+14=-f*cos(kappa)-(coori.y-yo)/f*(coori.x-xo)*sin(kappa)+(coori.y-yo)
14、*cos(kappa);A2*i+15=-(coori.x-xo);for (i=0;i6;i+)for (j=0;j6;j+)printf(“%f “,Aij);if (j%5=0printf(“n“);/计算 ATA 的各个元素for(i=0;i6;i+)for(j=0;j6;j+)ATAij=A0i*A0j+A1i*A1j+A2i*A2j+A3i*A3j+A4i*A4j+A5i*A5j+A6i*A6j+A7i*A7j;for (i=0;i6;i+)for (j=0;j6;j+)_ATAij=ATAij;printf(“%f “,ATAij);if (j%5=0printf(“n“);/计
15、算矩阵 L 的各个元素for(i=0;in;i+)L2*i0=coori.x+f*(R00*(coori.Xt-Xs)+R10*(coori.Yt-Ys)+R20*(coori.Zt-Zs)/(R02*(coori.Xt - Xs) + R12*(coori.Yt - Ys) + R22*(coori.Zt - Zs);L2*i+10=coori.y+f*(R01*(coori.Xt-Xs)+R11*(coori.Yt-Ys)+R21*(coori.Zt-Zs)/(R02*(coori.Xt - Xs) + R12*(coori.Yt - Ys) + R22*(coori.Zt - Zs);/
16、计算矩阵 ATL 的各个元素值for(i=0;i6;i+)ATLi0=A0i*L00+A1i*L10+A2i*L20+A3i*L30+A4i*L40+A5i*L50+A6i*L60+A7i*L70;/* for (i=0;i6;i+)for (j=0;j1;j+)ATLij=0;for (int k=0;k8;k+)ATLij+=Aki*Lkj;*/调用函数计算矩阵 ATA 的逆矩阵/ inverse(ATA);ContraryMatrix(*_ATA, *ATA, 6);for (i=0;i6;i+)for (j=0;j6;j+)printf(“%f “,ATAij);if (j%5=0/计
17、算矩阵 X 的各个元素值for(i=0;i6;i+)Xi0=ATAi0*ATL00+ATAi1*ATL10+ATAi2*ATL20+ATAi3*ATL30+ATAi4*ATL40+ATAi5*ATL50;printf(“%f “,Xi0);/ for (i=0;i6;i+)/ / for (j=0;j1;j+)/ / Xij=0;/ for (int k=0;k6;k+)/ / Xij+=ATAik*ATLkj;/ / / printf(“%f“,Xij);/ / DXs=X00;DYs=X10;DZs=X20;Dphi=X30;Domega=X40;Dkappa=X50;Xs+=DXs;Ys
18、+=DYs;Zs+=DZs;phi+=Dphi;omega+=Domega;kappa+=Dkappa;/计算矩阵 V 的各个元素for(i=0;in;i+)V2*i0=A2*i0*DXs+A2*i1*DYs+A2*i2*DZs+A2*i3*Dphi+A2*i4*Domega+A2*i5*Dkappa-L2*i0;V2*i+10=A2*i+10*DXs+A2*i+11*DYs+A2*i+12*DZs+A2*i+13*Dphi+A2*i+14*Domega+A2*i+15*Dkappa-L2*i+10;/* /像点坐标改正后的值for(i=0;in;i+)coori.x+=V2*i0;coori
19、.y+=V2*i+10;*/判断限差的条件while(!(fabs(Dphi) 0.1/60/180*PI /外方位元素计算完毕/有关精度的计算for(i=0;i6;i+)Qii=ATAii;m0=sqrt(V00*V00+V10*V10+V20*V20+V30*V30+V40*V40+V50*V50+V60*V60+V70*V70)/(2*n-6);/计算各未知数的中误差m1=sqrt(Q00)*m0;m2=sqrt(Q11)*m0;m3=sqrt(Q22)*m0;m4=sqrt(Q33)*m0;m5=sqrt(Q44)*m0;m6=sqrt(Q55)*m0;printf(“改正后的相对坐标
20、及其对应的地面点坐标（x,y,Xt,Yt,Zt ）:n“);for(i=0;in;i+)printf(“%lft%lft%lft%lft%lf“,(x)i,(y)i,coori.Xt,coori.Yt,coori.Zt);printf(“n“);printf(“旋转矩阵 R：n“);for(i=0;i3;i+)for(j=0;j3;j+)printf(“%lft“,Rij);printf(“n“);printf(“外方位元素(Xs,Ys,Zs,phi,omega,kappa) 及改正值： n“);printf(“%lf+-%lfn%lf+-%lfn%lf+-%lfn%lf+-%lfn%lf+-
21、%lfn%lf+-%lfn“,Xs,DXs,Ys,DYs,Zs,DZs,phi,Dphi,omega,Domega,kappa,Dkappa);printf(“单位权中误差 :n“);printf(“%0.9fn“,m0);printf(“外方位元素(Xs,Ys,Zs,phi,omega,kappa) 的中误差 :n“);printf(“%lfn%lfn%lfn%lfn%lfn%lfn“,m1,m2,m3,m4,m5,m6);/计算完毕，输出结果,以文件方式保存printf(“计算结果存储在文件中 n“);FILE *fp;fp=fopen(“计算结果.txt“,“w“);fprintf(fp
22、,“改正后的相对坐标及其对应的地面点坐标（x,y,Xt,Yt,Zt）:n“);for(i=0;in;i+)fprintf(fp,“%lft%lft%lft%lft%lf“,coori.x,coori.y,coori.Xt,coori.Yt,coori.Zt);fprintf(fp,“n“);fprintf(fp,“旋转矩阵 R：n“);for(i=0;i3;i+)for(j=0;j3;j+)fprintf(fp,“%lft“,Rij);fprintf(fp,“n“);fprintf(fp,“外方位元素(Xs,Ys,Zs,phi,omega,kappa)及改正值：n“);fprintf(fp,“%lf+-%lfn%lf+-%lfn%lf+-%lfn%lf+-%lfn%lf+-%lfn%lf+-%lfn“,Xs,DXs,Ys,DYs,Zs,DZs,phi,Dphi,omega,Domega,kappa,Dkappa);fprintf(fp,“单位权中误差:n“);fprintf(fp,“%0.9fn“,m0);fprintf(fp,“外方位元素(Xs,Ys,Zs,phi,omega,kappa)的中误差:n“);fprintf(fp,“%lfn%lfn%lfn%lfn%lfn%lfn“,m1,m2,m3,m4,m5,m6);fclose(fp);
