空间后方交会程序

第1页共8页一．实验目的：掌握摄影测量空间后方交会的原理，利用计算机编程语言实现空间后方交会外方位元素的解算。二．仪器用具及已知数据文件：计算机windowsxp系统，编程软件（VISUALC++6.0），地面控制点在摄影测量坐标系中的坐标及其像点坐标文件shuju.txt。三．实验内容：单张影像的空间后方交会：利用已知地面控制点数据及相应像点坐标根据共线方程反求影像的外方位元素。数学模型：共线条件方程式：)(3)(3)(3)(1)(1)(1ZsZcYsYbXsXaZsZcYsYbXsXafx)(3)(3)(3)(2)(2)(2ZsZcYsYbXsXaZsZcYsYbXsXafy求解过程：（1）获取已知数据。从航摄资料中查取平均航高与摄影机主距；获取控制点的地面测量坐标并转换为地面摄影测量坐标。（2）量测控制点的像点坐标并做系统改正。（3）确定未知数的初始值。在竖直摄影且地面控制点大致分布均匀的情况下，按如下方法确定初始值，即：nXXS0,nYYS0,nZmfZS0φ=ω=κ=0式中；m为摄影比例尺分母；n为控制点个数。（4）用三个角元素的初始值，计算个方向余弦，组成旋转矩阵R。（5）逐点计算像点坐标的近似值。利用未知数的近似值和控制点的地面坐标代入共线方程式，逐点计算像点坐标的近似值（x）、（y）。（6）逐点计算误差方程式的系数和常数项，组成误差方程式。（7）计算法方程的系数矩阵AAT和常数项lAT，组成法方程式。（8）解法方程，求得外方位元素的改正数dXs,SdY，sdZ，dφ,dω,dκ。（9）用前次迭代取得的近似值，加本次迭代的改正数，计算外方位元素的新值。第2页共8页（10）将求得的外方位元素改正数与规定的限差比较，若小于限差则迭代结束。否则用新的近似值重复（4）～（9），直到满足要求为止。四．实验结果：程序的源代码如下所示：#includestdio.h#includestdlib.h#includemalloc.h#includemath.h#includeconio.h#defineN4voidturn(double*A,doubleA2[],intm,intn)//计算矩阵的转置{inti,j;for(i=0;im;i++)for(j=0;jn;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;iam;i++)for(j=0;jbn;j++){u=i*bn+j;C[u]=0.0;for(l=0;lan;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));第3页共8页for(k=0;k=n-1;k++){d=0.0;for(i=k;in;i++)for(j=k;jn;j++){l=i*n+j;p=fabs(a[l]);if(pd){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;jn;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;in;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;jn;j++)if(j!=k){u=k*n+j;a[u]=a[u]*a[l];}第4页共8页for(i=0;in;i++)if(i!=k)for(j=0;jn;j++)if(j!=k){u=i*n+j;a[u]=a[u]-a[i*n+k]*a[k*n+j];}for(i=0;in;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;in;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;}voidprintmatrix(doubleM[],intm,intn)//矩阵的输出{inti,j;for(i=0;im;i++){for(j=0;jn;j++)printf(%.5f,M[i*n+j]);printf(\n);}第5页共8页printf(\n);}main()//主函数，空间后方交会的计算{FILE*fp;//定义一个文件指针fpintm,i,j=0;doublef,t,w,k,S1=0.0,S2=0.0,S3=0.0,x[N],y[N],x0[N],y0[N],X[N],Y[N],Z[N],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];if((fp=fopen(e:\\shuju.txt,r))==NULL)//使fp指向被打开的shuju.txt文件{//并判断文件是否打开成功printf(\nerroronopenshuju.txt\n);getch();exit(1);}for(i=0;iN;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\t\X\t\tY\t\tZ\t\t\n);//输出文件中的原始数据for(i=0;iN;i++)printf(%lf%lf%lf%lf%lf\n,x[i],y[i],X[i],Y[i],Z[i]);printf(\n请输入摄影机主距和摄影比例尺分母；f,m:);scanf(%lf,%lf,&f,&m);//输入f,mf=f/1000.0;for(i=0;iN;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(j3){printf(---------------------------------第%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);//计算旋转矩阵第6页共8页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;iN;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;iN;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);//计算误差方程的系数阵以及lA[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]=x[i]-x0[i];l[i*2+1]=y[i]-y0[i];}//printf(outputmatrix:A\n);//printmatrix(A,2*N,6);//printf(outputmatrix:l\n);//printmatrix(l,2*N,1);turn(A,AT,2*N,6);//printf(outputmatrix:AT\n);//printmatrix(AT,6,2*N);mulAB(AT,A,ATA,6,2*N,2*N,6);//间接平差法计算外方位元素,中间计算的矩阵可以根据需要//printf(outputmatrix:ATA\n);//选择性的输出，这里将其屏蔽，为了在打印输出结果的时候//printmatrix(ATA,6,6);//节约资源ATA_=inv(ATA,6);//printf(outputmatrix:ATA_\n);第7页共8页//printmatrix(ATA_,6,6);mulAB(AT,l,ATl,6,2*N,2*N,1);//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];printf(第%d次计算的外方位元素为：\n,++j);printf(Xs=%.5lf,Ys=%.5lf,Zs=%.5lf,t=%.5lf,w=%.5lf,k=%.5lf\n,Xs0,Ys0,Zs0,t,w,k);}printf(\n外