后方交会程序实现(c语言版)

#include<stdio.h>#include<stdlib.h>#include<malloc.h>#include<math.h>#include<conio.h>#define N 4#define T 1.41421void turn(double *A,double A2[],int m,int n) //计算矩阵的转置{ int i,j;for(i=0;i<m;i++)for(j=0;j<n;j++)A2[j*m+i]=A[i*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){printf("error!cannot do the multiplication.\n");return;}for(i=0;i<am;i++)for(j=0;j<bn;j++){u=i*bn+j;C[u]=0.0;for(l=0;l<an;l++)C[u]+=A[i*an+l]*B[l*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));for (k=0; k<=n-1; k++){d=0.0;for (i=k;i<n;i++)for (j=k;j<n;j++){ l=i*n+j;p=fabs(a[l]);if (p>d){d=p;is[k]=i;js[k]=j;}}if (d+1.0==1.0){ free(is);free(js);printf("error not inv\n");return NULL;}if (is[k]!=k)for (j=0;j<n;j++) { u=k*n+j;v=is[k]*n+j;p=a[u];a[u]=a[v];a[v]=p;}if (js[k]!=k)for (i=0;i<n;i++) { u=i*n+k;v=i*n+js[k];p=a[u];a[u]=a[v];a[v]=p;}l=k*n+k;a[l]=1.0/a[l];for (j=0;j<n;j++)if (j!=k){u=k*n+j;a[u]=a[u]*a[l];}for (i=0;i<n;i++)if (i!=k)for (j=0;j<n;j++)if (j!=k){ u=i*n+j;a[u]=a[u]-a[i*n+k]*a[k*n+j];}for (i=0;i<n;i++)if (i!=k){ u=i*n+k;a[u]=-a[u]*a[l];}}for (k=n-1;k>=0;k--){ if (js[k]!=k)for (j=0;j<=n-1;j++){ u=k*n+j;v=js[k]*n+j;p=a[u];a[u]=a[v];a[v]=p;}if (is[k]!=k)for (i=0;i<n;i++){ u=i*n+k;v=i*n+is[k];p=a[u];a[u]=a[v];a[v]=p;}}free(is);free(js);return a;}main()//主函数，空间后方交会的计算{FILE *fp; //定义一个文件指针long m;int i,j=0,it;double G[1000];doublef,t,w,k,limit=0,S1=0.0,S2=0.0,S3=0.0,x[N]={0},y[N]={0},x0[N]={0},y0[N]={0},X[N]={0},Y[N] ={0},Z[N]={0},Xs0,Ys0,Zs0;doublea[3],b[3],c[3],A[2*N*6],AT[6*2*N],ATA[6*6],*ATA_=NULL,l[2*N],ATl[6],V[6]={0};double F[6],Qx[6][6],Mi[6][6];if((fp=fopen("e:\\shuju.txt","r"))==NULL) //使fp指向被打开的shuju.txt 文件{ //并判断文件是否打开成功printf("\nerror on open shuju.txt\n");getch();exit(1);}for(i=0;i<N;i++)fscanf(fp,"%lf%lf%lf%lf%lf",&x[i],&y[i],&X[i],&Y[i],&Z[i]); //将文件中的数据赋给主函数定义的变量printf("原始数据:\n");printf("x\t\ty\t\tX\t\tY\t\tZ\t\t\n");//输出文件中的原始数据for(i=0;i<N;i++)printf("%.3lf\t\t%.3lf\t\t%.3lf\t%.3lf\t%.3lf\n",x[i],y[i],X[i],Y[i],Z[i]);printf("\n请输入摄影机主距和摄影比例尺分母；f,m:");scanf("%lf%ld",&f,&m); //输入f,mf=f/1000.0;for(i=0;i<N;i++){ x[i]/=1000.0;y[i]/=1000.0;S1+=X[i];S2+=Y[i];S3+=Z[i];}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);a[0]=cos(t)*cos(k)-sin(t)*sin(w)*sin(k);a[1]=-cos(t)*sin(k)-sin(t)*sin(w)*cos(k);a[2]=-sin(t)*cos(w);b[0]=cos(w)*sin(k);b[1]=cos(w)*cos(k);b[2]=-sin(w);c[0]=sin(t)*cos(k)+cos(t)*sin(w)*sin(k);c[1]=-sin(t)*sin(k)+cos(t)*sin(w)*cos(k);c[2]=cos(t)*cos(w); //计算旋转矩阵for(i=0;i<N;i++){x0[i]=-f*(a[0]*(X[i]-Xs0)+b[0]*(Y[i]-Ys0)+c[0]*(Z[i]-Zs0))/(a[2]*(X[i]-Xs0)+b[2]*(Y[i]-Ys0)+c [2]*(Z[i]-Zs0));y0[i]=-f*(a[1]*(X[i]-Xs0)+b[1]*(Y[i]-Ys0)+c[1]*(Z[i]-Zs0))/(a[2]*(X[i]-Xs0)+b[2]*(Y[i]-Ys0)+c [2]*(Z[i]-Zs0));//计算像点坐标近似值G[i]=a[2]*(X[i]-Xs0)+b[2]*(Y[i]-Ys0)+c[2]*(Z[i]-Zs0);}for(i=0;i<N;i++){A[i*12+0]=(a[0]*f+a[2]*x[i])/G[i];A[i*12+1]=(b[0]*f+b[2]*x[i])/G[i];A[i*12+2]=(c[0]*f+c[2]*x[i])/G[i];A[i*12+3]=y[i]*sin(w)-(x[i]*(x[i]*cos(k)-y[i]*sin(k))/f+f*cos(k))*cos(w);A[i*12+4]=-f*sin(k)-x[i]*(x[i]*sin(k)+y[i]*cos(k))/f;A[i*12+5]=y[i];A[i*12+6]=(a[1]*f+a[2]*y[i])/G[i];A[i*12+7]=(b[1]*f+b[2]*y[i])/G[i];A[i*12+8]=(c[1]*f+c[2]*y[i])/G[i];A[i*12+9]=-x[i]*sin(w)-(y[i]*(x[i]*cos(k)-y[i]*sin(k))/f-f*sin(k))*cos(w);A[i*12+10]=-f*cos(k)-y[i]*(x[i]*sin(k)+y[i]*cos(k))/f;A[i*12+11]=-x[i];l[i*2+0]=x0[i]-x[i];l[i*2+1]=y0[i]-y[i]; //计算误差方程的系数阵以及lx,ly}// printf("output matrix: A\n");// printmatrix(A,2*N,6);// printf("output matrix: l\n");// printmatrix(l,2*N,1);turn(A,A T,2*N,6); //计算AT// printf("output matrix: AT\n");// 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;it<36;it++){p=it%6;if(it%6==0){cnt++;}Qx[cnt][p++]=A TA_[it];}for(int it=0;it<6;it++){for(int jt=0;jt<6;jt++){if(it!=jt){Qx[it][jt]=0;//提取Qx的主对角线元素=Qii}// printf("%-10.3lf ",Qx[it][jt]);}// printf("\n");}mulAB(A T,l,ATl,6,2*N,2*N,1); //计算常数项ATL// printf("outpit matrinx: ATl\n");// printmatrix(ATl,6,1);mulAB(A TA_,ATl,V,6,6,6,1); //解法方程，求改正数，// printf("output matrix: V\n");// printmatrix(V,6,1);Xs0+=V[0];Ys0+=V[1];Zs0+=V[2];t+=V[3];w+=V[4];k+=V[5];for(i=0;i<6;i++){F[i]=V[i]/T;//m0}printf("第%d次计算的外方位元素为：\n",++j);printf("Xs=%.5lf\tYs=%.5lf\tZs=%.5lf\nt=%.5lf\tw=%.5lf\tk=%.5lf\n",Xs0,Ys0,Zs0,t,w,k);if(Xs0-limit<=0.0001&&Xs0-limit>=-0.0001){ //控制迭代次数break;}limit=Xs0;}printf("\n外方位元素为：\n");printf("Xs=%.5lf\tYs=%.5lf\tZs=%.5lf\nt=%.5lf\tw=%.5lf\tk=%.5lf\n",Xs0,Ys0,Zs0,t,w,k);printf("\n中误差为:\n");for(i=0;i<6;i++){for(j=0;j<6;j++){Mi[i][j]=F[i]*(sqrt(Qx[i][j]));//mi=m0*Qii开根号printf("%-13.10lf",Mi[i][j]);}printf("\n");}fclose(fp);return 0;}。