C語言空間后方交會源代碼

1、#include<stdio.h>#include<math.h>#define n 4 /控制點個數(shù)#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+)/構(gòu)造高斯矩陣/ 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+)/消去對角線以下的數(shù)據(jù)/ 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-) / 消去對角線以上的數(shù)據(jù)/ 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+)/將對角線上數(shù)據(jù)化為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; /內(nèi)方位元素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; /循環(huán)變量double Xs,Ys,Zs; /外方位線元素double phi,omega,kappa; /外方位角元素double R33; /旋轉(zhuǎn)矩陣Rdouble (x)n,(y)n; /控制點像點坐標的近似值 double A86; /誤差方程中的矩陣Adouble ATA66; /矩陣A的轉(zhuǎn)置矩陣與A的乘積double L81;double ATL61; /矩陣A的轉(zhuǎn)置矩陣與L的乘積double _ATA66;double X61; /未知數(shù)矩陣double V81; /改正數(shù)矩陣double DXs,DYs,DZs,Dphi,Domega,Dkappa; /6個

9、外方位元素的改正值/與精度計算有關(guān)的量double m0; /單位權(quán)中誤差double Q66; /協(xié)方差矩陣double m1,m2,m3,m4,m5,m6; /未知數(shù)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 /計算旋轉(zhuǎn)矩陣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);/將未知數(shù)的近似值代

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;*/調(diào)用函數(shù)計算矩陣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); /外方位元素計算完畢/有關(guān)精度的計算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);/計算各未知數(shù)的中誤差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("旋轉(zhuǎn)矩陣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("單位權(quán)中誤差: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);/計算完畢，輸出結(jié)果,以文件方式保存printf("計