C语言空间后方交会源代码

1、#include<stdio.h>#include<math.h>#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;i<6;i+)/构造高斯矩阵/ for(j=0;j<6;j+)/ qij=cij;/ for(i=

2、0;i<6;i+)/ for(j=6;j<12;j+)/ / if(i+6=j)/ qij=1;/ else/ qij=0;/ / for(h=k=0;k<n-1;k+,h+)/消去对角线以下的数据/ for(i=k+1;i<6;i+)/ / if(qih=0)/ continue;/ p=qkh/qih;/ /p=qih/qkh;/ for(j=0;j<12;j+)/ / qij*=p;/ qij-=qkj;/ / / for(h=k=5;k>0;k-,h-) / 消去对角线以上的数据/ for(i=k-1;i>=0;i-)/ / if(qih=0)

3、/ continue;/ p=qkh/qih;/ /p=qih/qkh;/ for(j=11;j>0;j-)/ / qij*=p;/ qij-=qkj;/ / / for(i=0;i<6;i+)/将对角线上数据化为1/ / p=1.0/qii;/ for(j=0;j<12;j+)/ qij*=p;/ / for(i=0;i<6;i+) /提取逆矩阵/ for(j=0;j<n;j+)/ / cij=qij+6;/ / void ContraryMatrix(double *const pMatrix, double *const _pMatrix, const in

4、t &dim) int ii,jj,kk;int flag=0;double *tMatrix = new double2*dim*dim;for (int i=0; i<dim; i+)for (int j=0; j<dim; j+)tMatrixi*dim*2+j = pMatrixi*dim+j; for (i=0; i<dim; i+)for (int j=dim; j<dim*2; j+)tMatrixi*dim*2+j = 0.0;tMatrixi*dim*2+dim+i = 1.0; /Initialization over!for (i=0; i

5、<dim; i+)/Process Colsdouble base = tMatrixi*dim*2+i;if (fabs(base) < 1E-300)for (ii=i;ii<dim;ii+)if(tMatrixii*dim*2+i!=0)flag=1;for (jj=0;jj<2*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;

6、j<dim; j+)/rowif (j = i) continue;double times = tMatrixj*dim*2+i/base;for (int k=0; k<dim*2; k+)/col tMatrixj*dim*2+k = tMatrixj*dim*2+k - times*tMatrixi*dim*2+k; for (int k=0; k<dim*2; k+)tMatrixi*dim*2+k /= base;for (i=0; i<dim; i+)for (int j=0; j<dim; j+)_pMatrixi*dim+j = tMatrixi

7、*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.08615,-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;

8、 int i,j; /循环变量double Xs,Ys,Zs; /外方位线元素double phi,omega,kappa; /外方位角元素double R33; /旋转矩阵Rdouble (x)n,(y)n; /控制点像点坐标的近似值 double A86; /误差方程中的矩阵Adouble ATA66; /矩阵A的转置矩阵与A的乘积double L81;double ATL61; /矩阵A的转置矩阵与L的乘积double _ATA66;double X61; /未知数矩阵double V81; /改正数矩阵double DXs,DYs,DZs,Dphi,Domega,Dkappa; /6个

9、外方位元素的改正值/与精度计算有关的量double m0; /单位权中误差double Q66; /协方差矩阵double m1,m2,m3,m4,m5,m6; /未知数Xs,Ys,Zs,phi,omega,kappa的中误差矩阵 Xs=0;Ys=0;for(i=0;i<n;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(kap

10、pa);R01=-cos(phi)*sin(kappa)-sin(phi)*sin(omega)*cos(kappa);R02=-sin(phi)*cos(omega);R10=cos(omega)*sin(kappa);R11=cos(omega)*cos(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);/将未知数的近似值代

11、人共线方程式，计算(x),(y)for(i=0;i<n;i+)(x)i=-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); (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;i<n;i+)

12、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 - 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.

13、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*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.

14、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);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

15、)*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)*cos(kappa);A2*i+15=-(coori.x-xo);for (i=0;i<6;i+)for (j=0;j<6;j+)printf("%f ",Aij);if (j%5=0&&j!=0)printf("n");printf("'n&

16、quot;);/计算ATA的各个元素for(i=0;i<6;i+)for(j=0;j<6;j+)ATAij=A0i*A0j+A1i*A1j+A2i*A2j+A3i*A3j+A4i*A4j+A5i*A5j+A6i*A6j+A7i*A7j; for (i=0;i<6;i+)for (j=0;j<6;j+)_ATAij=ATAij;printf("%f ",ATAij);if (j%5=0&&j!=0)printf("n");printf("n");/计算矩阵L的各个元素for(i=0;i<n;

17、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);/计算矩阵ATL的各个元素值for(i=0;

18、i<6;i+)ATLi0=A0i*L00+A1i*L10+A2i*L20+A3i*L30+A4i*L40+A5i*L50+A6i*L60+A7i*L70;/*for (i=0;i<6;i+)for (j=0;j<1;j+)ATLij=0;for (int k=0;k<8;k+)ATLij+=Aki*Lkj;*/调用函数计算矩阵ATA的逆矩阵/inverse(ATA); ContraryMatrix(*_ATA, *ATA, 6);for (i=0;i<6;i+)for (j=0;j<6;j+)printf("%f ",ATAij);if

19、(j%5=0&&j!=0)printf("n");/计算矩阵X的各个元素值for(i=0;i<6;i+)Xi0=ATAi0*ATL00+ATAi1*ATL10+ATAi2*ATL20+ATAi3*ATL30+ATAi4*ATL40+ATAi5*ATL50;printf("%f ",Xi0);/ for (i=0;i<6;i+)/ / for (j=0;j<1;j+)/ / Xij=0;/ for (int k=0;k<6;k+)/ / Xij+=ATAik*ATLkj;/ / / printf("%f&q

20、uot;,Xij);/ / DXs=X00;DYs=X10;DZs=X20;Dphi=X30;Domega=X40;Dkappa=X50;Xs+=DXs;Ys+=DYs;Zs+=DZs;phi+=Dphi;omega+=Domega;kappa+=Dkappa;/计算矩阵V的各个元素for(i=0;i<n;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

21、+14*Domega+A2*i+15*Dkappa-L2*i+10;/*/像点坐标改正后的值for(i=0;i<n;i+)coori.x+=V2*i0;coori.y+=V2*i+10;*/判断限差的条件while(!(fabs(Dphi) < 0.1/60/180*PI && fabs(Domega) < 0.1/60/180*PI && fabs(Dkappa) < 0.1/60/180*PI); /外方位元素计算完毕/有关精度的计算for(i=0;i<6;i+)Qii=ATAii;m0=sqrt(V00*V00+V10*V10

22、+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("改正后的相对坐标及其对应的地面点坐标（x,y,Xt,Yt,Zt）:n");for(i=0;i<n;i+)printf("%lft%lft%lft%lft%lf",(x)i,(y)i,coori.Xt,coor

23、i.Yt,coori.Zt);printf("n");printf("旋转矩阵R：n");for(i=0;i<3;i+)for(j=0;j<3;j+)printf("%lft",Rij);printf("n");printf("外方位元素(Xs,Ys,Zs,phi,omega,kappa)及改正值：n");printf("%lf+-%lfn%lf+-%lfn%lf+-%lfn%lf+-%lfn%lf+-%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("计