后方交会程序实现c语言版.docx
- 文档编号:30312849
- 上传时间:2023-08-13
- 格式:DOCX
- 页数:10
- 大小:16.27KB
后方交会程序实现c语言版.docx
《后方交会程序实现c语言版.docx》由会员分享,可在线阅读,更多相关《后方交会程序实现c语言版.docx(10页珍藏版)》请在冰豆网上搜索。
后方交会程序实现c语言版
#include
#include
#include
#include
#include
#defineN4
#defineT1.41421
voidturn(double*A,doubleA2[],intm,intn)//计算矩阵的转置
{inti,j;
for(i=0;i for(j=0;j A2[j*m+i]=A[i*n+j]; } voidmulAB(double*A,double*B,double*C,intam,intan,intbm,intbn)//计算两矩阵相乘 { inti,j,l,u; if(an! =bm) { printf("error! cannotdothemultiplication.\n"); return; } for(i=0;i for(j=0;j { u=i*bn+j; C[u]=0.0; for(l=0;l C[u]+=A[i*an+l]*B[l*bn+j]; } return; } double*inv(double*a,intn)//计算矩阵的逆,本程序的难点,采用高斯约旦全选主元法{ int*is,*js,i,j,k,l,u,v; doubled,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 for(j=k;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("errornotinv\n"); returnNULL; } if(is[k]! =k) for(j=0;j v=is[k]*n+j; p=a[u]; a[u]=a[v]; a[v]=p; } if(js[k]! =k) for(i=0;i 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 if(j! =k) { u=k*n+j; a[u]=a[u]*a[l]; } for(i=0;i if(i! =k) for(j=0;j if(j! =k) {u=i*n+j; a[u]=a[u]-a[i*n+k]*a[k*n+j]; } for(i=0;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 {u=i*n+k; v=i*n+is[k]; p=a[u]; a[u]=a[v]; a[v]=p; } } free(is); free(js); returna; } main()//主函数,空间后方交会的计算{ FILE*fp;//定义一个文件指针 longm; inti,j=0,it; doubleG[1000]; double f,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; double a[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}; doubleF[6],Qx[6][6],Mi[6][6]; if((fp=fopen("e: \\shuju.txt","r"))==NULL)//使fp指向被打开的shuju.txt文件 {//并判断文件是否打开成功 printf("\nerroronopenshuju.txt\n"); getch(); exit (1); } for(i=0;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 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,m f=f/1000.0; for(i=0;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 { 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 { 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("outputmatrix: A\n"); //printmatrix(A,2*N,6); //printf("outputmatrix: l\n"); //printmatrix(l,2*N,1); turn(A,AT,2*N,6);//计算AT //printf("outputmatrix: AT\n"); //printmatrix(AT,6,2*N); mulAB(AT,A,ATA,6,2*N,2*N,6);//计算ATA,组法方程 ATA_=inv(ATA,6);//计算ATA的逆,中间量 intp; intcnt=-1; for(it=0;it<36;it++){ p=it%6; if(it%6==0){ cnt++; } Qx[cnt][p++]=ATA_[it]; } for(intit=0;it<6;it++){ for(intjt=0;jt<6;jt++){ if(it! =jt){ Qx[it][jt]=0;//提取Qx的主对角线元素=Qii } //printf("%-10.3lf",Qx[it][jt]); } //printf("\n"); } mulAB(AT,l,ATl,6,2*N,2*N,1);//计算常数项ATL //printf("outpitmatrinx: ATl\n"); //printmatrix(ATl,6,1); mulAB(ATA_,ATl,V,6,6,6,1);//解法方程,求改正数, //printf("outputmatrix: 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); return0; }
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 后方 交会 程序 实现 语言版