后方交会程序实现c语言版.docx

1、后方交会程序实现c语言版#include #include #include #include #include #define N 4 #define T 1.41421 void turn(double *A,double A2,int m,int n) /计算矩阵的转置 int i,j; for(i=0;im;i+) for(j=0;jn;j+) A2j*m+i=Ai*n+j; void mulAB(double *A,double *B,double *C,int am,int an,int bm,int bn) /计算两矩阵相乘 int i,j,l,u; if(an!=bm) pri

2、ntf(error!cannot do the multiplication.n); return; for(i=0;iam;i+) for(j=0;jbn;j+) u=i*bn+j; Cu=0.0; for(l=0;lan;l+) Cu+=Ai*an+l*Bl*bn+j; return; double *inv(double *a,int n) /计算矩阵的逆,本程序的难点,采用高斯约旦全选主元法 int *is,*js,i,j,k,l,u,v; double d,p; is=(int*)malloc(n*sizeof(int); js=(int*)malloc(n*sizeof(int);

3、 for (k=0; k=n-1; k+) d=0.0; for (i=k;in;i+) for (j=k;jd) d=p; isk=i; jsk=j; if (d+1.0=1.0) free(is); free(js); printf(error not invn); return NULL; if (isk!=k) for (j=0;jn;j+) u=k*n+j; v=isk*n+j; p=au; au=av; av=p; if (jsk!=k) for (i=0;in;i+) u=i*n+k; v=i*n+jsk; p=au; au=av; av=p; l=k*n+k; al=1.0/a

4、l; for (j=0;jn;j+) if (j!=k) u=k*n+j; au=au*al; for (i=0;in;i+) if (i!=k) for (j=0;jn;j+) if (j!=k) u=i*n+j; au=au-ai*n+k*ak*n+j; for (i=0;i=0;k-) if (jsk!=k) for (j=0;j=n-1;j+) u=k*n+j; v=jsk*n+j; p=au; au=av; av=p; if (isk!=k) for (i=0;in;i+) u=i*n+k; v=i*n+isk; p=au; au=av; av=p; free(is); free(j

5、s); return a; main()/主函数,空间后方交会的计算 FILE *fp; /定义一个文件指针 long m; int i,j=0,it; double G1000; double f,t,w,k,limit=0,S1=0.0,S2=0.0,S3=0.0,xN=0,yN=0,x0N=0,y0N=0,XN=0,YN =0,ZN=0,Xs0,Ys0,Zs0; double a3,b3,c3,A2*N*6,AT6*2*N,ATA6*6,*ATA_=NULL,l2*N,ATl6,V6=0; double F6,Qx66,Mi66; if(fp=fopen(e:shuju.txt,r)=N

6、ULL) /使fp指向被打开的shuju.txt 文件 /并判断文件是否打开成功 printf(nerror on open shuju.txtn); getch(); exit(1); for(i=0;iN;i+) fscanf(fp,%lf%lf%lf%lf%lf,&xi,&yi,&Xi,&Yi,&Zi); /将文件中的数据赋给主函数定义的变量 printf(原始数据:n); printf(xttyttXttYttZttn);/输出文件中的原始数据 for(i=0;iN;i+) printf(%.3lftt%.3lftt%.3lft%.3lft%.3lfn,xi,yi,Xi,Yi,Zi);

7、 printf(n请输入摄影机主距和摄影比例尺分母;f,m:); scanf(%lf%ld,&f,&m); /输入f,m f=f/1000.0; for(i=0;iN;i+) xi/=1000.0; yi/=1000.0; S1+=Xi; S2+=Yi; S3+=Zi; Xs0=S1/N; Ys0=S2/N; Zs0=m*f+S3/N; t=0.0;w=0.0;k=0.0; /计算外方位元素的初始值 while(1) printf(n-第%d次计算-n,j+1); a0=cos(t)*cos(k)-sin(t)*sin(w)*sin(k); a1=-cos(t)*sin(k)-sin(t)*s

8、in(w)*cos(k); a2=-sin(t)*cos(w); b0=cos(w)*sin(k); b1=cos(w)*cos(k); b2=-sin(w); c0=sin(t)*cos(k)+cos(t)*sin(w)*sin(k); c1=-sin(t)*sin(k)+cos(t)*sin(w)*cos(k); c2=cos(t)*cos(w); /计算旋转矩阵 for(i=0;iN;i+) x0i=-f*(a0*(Xi-Xs0)+b0*(Yi-Ys0)+c0*(Zi-Zs0)/(a2*(Xi-Xs0)+b2*(Yi-Ys0)+c 2*(Zi-Zs0); y0i=-f*(a1*(Xi-X

9、s0)+b1*(Yi-Ys0)+c1*(Zi-Zs0)/(a2*(Xi-Xs0)+b2*(Yi-Ys0)+c 2*(Zi-Zs0); /计算像点坐标近似值 Gi=a2*(Xi-Xs0)+b2*(Yi-Ys0)+c2*(Zi-Zs0); for(i=0;iN;i+) Ai*12+0=(a0*f+a2*xi)/Gi; Ai*12+1=(b0*f+b2*xi)/Gi; Ai*12+2=(c0*f+c2*xi)/Gi; Ai*12+3=yi*sin(w)-(xi*(xi*cos(k)-yi*sin(k)/f+f*cos(k)*cos(w); Ai*12+4=-f*sin(k)-xi*(xi*sin(k

10、)+yi*cos(k)/f; Ai*12+5=yi; Ai*12+6=(a1*f+a2*yi)/Gi; Ai*12+7=(b1*f+b2*yi)/Gi; Ai*12+8=(c1*f+c2*yi)/Gi; Ai*12+9=-xi*sin(w)-(yi*(xi*cos(k)-yi*sin(k)/f-f*sin(k)*cos(w); Ai*12+10=-f*cos(k)-yi*(xi*sin(k)+yi*cos(k)/f; Ai*12+11=-xi; li*2+0=x0i-xi; li*2+1=y0i-yi; /计算误差方程的系数阵以及lx,ly / printf(output matrix: An

11、); / printmatrix(A,2*N,6); / printf(output matrix: ln); / printmatrix(l,2*N,1); turn(A,A T,2*N,6); /计算AT / printf(output matrix: ATn); / printmatrix(AT,6,2*N); mulAB(AT,A,ATA,6,2*N,2*N,6); /计算ATA,组法方程 ATA_=inv(ATA,6); /计算ATA的逆,中间量 int p; int cnt=-1; for(it=0;it36;it+) p=it%6; if(it%6=0) cnt+; Qxcntp

12、+=A TA_it; for(int it=0;it6;it+) for(int jt=0;jt6;jt+) if(it!=jt) Qxitjt=0;/提取Qx的主对角线元素=Qii / printf(%-10.3lf ,Qxitjt); / printf(n); mulAB(A T,l,ATl,6,2*N,2*N,1); /计算常数项ATL / printf(outpit matrinx: ATln); / printmatrix(ATl,6,1); mulAB(A TA_,ATl,V,6,6,6,1); /解法方程,求改正数, / printf(output matrix: Vn); /

13、printmatrix(V,6,1); Xs0+=V0; Ys0+=V1; Zs0+=V2; t+=V3; w+=V4; k+=V5; for(i=0;i6;i+) Fi=Vi/T;/m0 printf(第%d次计算的外方位元素为:n,+j); printf(Xs=%.5lftYs=%.5lftZs=%.5lfnt=%.5lftw=%.5lftk=%.5lfn,Xs0,Ys0,Zs0,t,w,k); if(Xs0-limit=-0.0001) /控制迭代次数 break; limit=Xs0; printf(n外方位元素为:n); printf(Xs=%.5lftYs=%.5lftZs=%.5lfnt=%.5lftw=%.5lftk=%.5lfn,Xs0,Ys0,Zs0,t,w,k); printf(n中误差为:n); for(i=0;i6;i+) for(j=0;j6;j+) Miij=Fi*(sqrt(Qxij);/mi=m0*Qii开根号 printf(%-13.10lf,Miij); printf(n); fclose(fp); return 0;