C语言空间后方交会源代码.docx
- 文档编号:5876893
- 上传时间:2023-05-09
- 格式:DOCX
- 页数:16
- 大小:17.97KB
C语言空间后方交会源代码.docx
《C语言空间后方交会源代码.docx》由会员分享,可在线阅读,更多相关《C语言空间后方交会源代码.docx(16页珍藏版)》请在冰点文库上搜索。
C语言空间后方交会源代码
#include
#include
#definen4//控制点个数
#definePI3.14159265
structcoordinate
{
doublex;//像点坐标
doubley;
doubleXt;//控制点坐标
doubleYt;
doubleZt;
};
//voidinverse(doublec[6][6])//矩阵求逆
//{
//inti,j,h,k;
//doublep;
//doubleq[6][12];
//for(i=0;i<6;i++)//构造高斯矩阵
//for(j=0;j<6;j++)
//q[i][j]=c[i][j];
//for(i=0;i<6;i++)
//for(j=6;j<12;j++)
//{
//if(i+6==j)
//q[i][j]=1;
//else
//q[i][j]=0;
//}
//for(h=k=0;k //for(i=k+1;i<6;i++) //{ //if(q[i][h]==0) //continue; //p=q[k][h]/q[i][h]; ////p=q[i][h]/q[k][h]; //for(j=0;j<12;j++) //{ //q[i][j]*=p; //q[i][j]-=q[k][j]; //} //} //for(h=k=5;k>0;k--,h--)//消去对角线以上的数据 //for(i=k-1;i>=0;i--) //{ //if(q[i][h]==0) //continue; //p=q[k][h]/q[i][h]; ////p=q[i][h]/q[k][h]; //for(j=11;j>0;j--) //{ //q[i][j]*=p; //q[i][j]-=q[k][j]; //} //} //for(i=0;i<6;i++)//将对角线上数据化为1 //{ //p=1.0/q[i][i]; //for(j=0;j<12;j++) //q[i][j]*=p; //} //for(i=0;i<6;i++)//提取逆矩阵 //for(j=0;j //{ //c[i][j]=q[i][j+6]; //} //} voidContraryMatrix(double*constpMatrix,double*const_pMatrix,constint&dim) { intii,jj,kk; intflag=0; double*tMatrix=newdouble[2*dim*dim]; for(inti=0;i for(intj=0;j tMatrix[i*dim*2+j]=pMatrix[i*dim+j]; } for(i=0;i for(intj=dim;j tMatrix[i*dim*2+j]=0.0; tMatrix[i*dim*2+dim+i]=1.0; } //Initializationover! for(i=0;i { doublebase=tMatrix[i*dim*2+i]; if(fabs(base)<1E-300) { for(ii=i;ii { if(tMatrix[ii*dim*2+i]! =0) { flag=1; for(jj=0;jj<2*dim;jj++) { tMatrix[i*dim*2+jj]+=tMatrix[ii*dim*2+jj]; } } } base=tMatrix[i*dim*2+i]; if(flag==0) { printf("求逆矩阵过程中被零除,无法求解! "); } //exit(0); } for(intj=0;j { if(j==i)continue; doubletimes=tMatrix[j*dim*2+i]/base; for(intk=0;k { tMatrix[j*dim*2+k]=tMatrix[j*dim*2+k]-times*tMatrix[i*dim*2+k]; } } for(intk=0;k tMatrix[i*dim*2+k]/=base; } } for(i=0;i { for(intj=0;j _pMatrix[i*dim+j]=tMatrix[i*dim*2+j+dim]; } delete[]tMatrix; } voidmain() { doublef,xo,yo;//内方位元素 intm;//摄影比例尺分母 f=0.15324; xo=0;yo=0; m=50000; structcoordinatecoor[n]={{-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}}; inti,j;//循环变量 doubleXs,Ys,Zs;//外方位线元素 doublephi,omega,kappa;//外方位角元素 doubleR[3][3];//旋转矩阵R double(x)[n],(y)[n];//控制点像点坐标的近似值 doubleA[8][6];//误差方程中的矩阵A doubleATA[6][6];//矩阵A的转置矩阵与A的乘积 doubleL[8][1]; doubleATL[6][1];//矩阵A的转置矩阵与L的乘积 double_ATA[6][6]; doubleX[6][1];//未知数矩阵 doubleV[8][1];//改正数矩阵 doubleDXs,DYs,DZs,Dphi,Domega,Dkappa;//6个外方位元素的改正值 //与精度计算有关的量 doublem0;//单位权中误差 doubleQ[6][6];//协方差矩阵 doublem1,m2,m3,m4,m5,m6;//未知数Xs,Ys,Zs,phi,omega,kappa的中误差矩阵 Xs=0;Ys=0; for(i=0;i { Xs+=coor[i].Xt; Ys+=coor[i].Yt; } //外方位元素的初始值 Xs/=n; Ys/=n; Zs=f*m; phi=0; omega=0; kappa=0;//在竖直摄影情况下 do { //计算旋转矩阵R的各个元素 R[0][0]=cos(phi)*cos(kappa)-sin(phi)*sin(omega)*sin(kappa); R[0][1]=-cos(phi)*sin(kappa)-sin(phi)*sin(omega)*cos(kappa); R[0][2]=-sin(phi)*cos(omega); R[1][0]=cos(omega)*sin(kappa); R[1][1]=cos(omega)*cos(kappa); R[1][2]=-sin(omega); R[2][0]=sin(phi)*cos(kappa)+cos(phi)*sin(omega)*sin(kappa); R[2][1]=-sin(phi)*sin(kappa)+cos(phi)*sin(omega)*cos(kappa); R[2][2]=cos(phi)*cos(omega); //将未知数的近似值代人共线方程式,计算(x),(y) for(i=0;i { (x)[i]=-f*(R[0][0]*(coor[i].Xt-Xs)+R[1][0]*(coor[i].Yt-Ys)+R[2][0]*(coor[i].Zt-Zs))/(R[0][2]*(coor[i].Xt-Xs)+R[1][2]*(coor[i].Yt-Ys)+R[2][2]*(coor[i].Zt-Zs)); (y)[i]=-f*(R[0][1]*(coor[i].Xt-Xs)+R[1][1]*(coor[i].Yt-Ys)+R[2][1]*(coor[i].Zt-Zs))/(R[0][2]*(coor[i].Xt-Xs)+R[1][2]*(coor[i].Yt-Ys)+R[2][2]*(coor[i].Zt-Zs)); } //计算矩阵A的各个元素 for(i=0;i { A[2*i][0]=(R[0][0]*f+R[0][2]*(coor[i].x-xo))/(R[0][2]*(coor[i].Xt-Xs)+R[1][2]*(coor[i].Yt-Ys)+R[2][2]*(coor[i].Zt-Zs)); A[2*i][1]=(R[1][0]*f+R[1][2]*(coor[i].x-xo))/(R[0][2]*(coor[i].Xt-Xs)+R[1][2]*(coor[i].Yt-Ys)+R[2][2]*(coor[i].Zt-Zs)); A[2*i][2]=(R[2][0]*f+R[2][2]*(coor[i].x-xo))/(R[0][2]*(coor[i].Xt-Xs)+R[1][2]*(coor[i].Yt-Ys)+R[2][2]*(coor[i].Zt-Zs)); A[2*i][3]=(coor[i].y-yo)*sin(omega)-((coor[i].x-xo)/f*((coor[i].x-xo)*cos(kappa)-(coor[i].y-yo)*sin(kappa))+f*cos(kappa))*cos(omega); A[2*i][4]=-f*sin(kappa)-(coor[i].x-xo)/f*((coor[i].x-xo)*sin(kappa)+(coor[i].y-yo)*cos(kappa)); A[2*i][5]=coor[i].y-yo; A[2*i+1][0]=(R[0][1]*f+R[0][2]*(coor[i].y-yo))/(R[0][2]*(coor[i].Xt-Xs)+R[1][2]*(coor[i].Yt-Ys)+R[2][2]*(coor[i].Zt-Zs)); A[2*i+1][1]=(R[1][1]*f+R[1][2]*(coor[i].y-yo))/(R[0][2]*(coor[i].Xt-Xs)+R[1][2]*(coor[i].Yt-Ys)+R[2][2]*(coor[i].Zt-Zs)); A[2*i+1][2]=(R[2][1]*f+R[2][2]*(coor[i].y-yo))/(R[0][2]*(coor[i].Xt-Xs)+R[1][2]*(coor[i].Yt-Ys)+R[2][2]*(coor[i].Zt-Zs)); A[2*i+1][3]=-(coor[i].x-xo)*sin(omega)-((coor[i].y-yo)/f*((coor[i].x-xo)*cos(kappa)-(coor[i].y-yo)*sin(kappa))-f*sin(kappa))*cos(omega); A[2*i+1][4]=-f*cos(kappa)-(coor[i].y-yo)/f*((coor[i].x-xo)*sin(kappa)+(coor[i].y-yo)*cos(kappa)); A[2*i+1][5]=-(coor[i].x-xo); } for(i=0;i<6;i++) { for(j=0;j<6;j++) { printf("%f",A[i][j]); if(j%5==0&&j! =0) { printf("\n"); } } } printf("'\n"); //计算ATA的各个元素 for(i=0;i<6;i++) for(j=0;j<6;j++) ATA[i][j]=A[0][i]*A[0][j]+A[1][i]*A[1][j]+A[2][i]*A[2][j]+A[3][i]*A[3][j]+A[4][i]*A[4][j] +A[5][i]*A[5][j]+A[6][i]*A[6][j]+A[7][i]*A[7][j]; for(i=0;i<6;i++) { for(j=0;j<6;j++) { _ATA[i][j]=ATA[i][j]; printf("%f",ATA[i][j]); if(j%5==0&&j! =0) { printf("\n"); } } } printf("\n"); //计算矩阵L的各个元素 for(i=0;i { L[2*i][0]=coor[i].x+f*(R[0][0]*(coor[i].Xt-Xs)+R[1][0]*(coor[i].Yt-Ys)+R[2][0]*(coor[i].Zt-Zs))/(R[0][2]*(coor[i].Xt-Xs)+R[1][2]*(coor[i].Yt-Ys)+R[2][2]*(coor[i].Zt-Zs)); L[2*i+1][0]=coor[i].y+f*(R[0][1]*(coor[i].Xt-Xs)+R[1][1]*(coor[i].Yt-Ys)+R[2][1]*(coor[i].Zt-Zs))/(R[0][2]*(coor[i].Xt-Xs)+R[1][2]*(coor[i].Yt-Ys)+R[2][2]*(coor[i].Zt-Zs)); } //计算矩阵ATL的各个元素值 for(i=0;i<6;i++) ATL[i][0]=A[0][i]*L[0][0]+A[1][i]*L[1][0]+A[2][i]*L[2][0]+A[3][i]*L[3][0]+A[4][i]*L[4][0] +A[5][i]*L[5][0]+A[6][i]*L[6][0]+A[7][i]*L[7][0]; /*for(i=0;i<6;i++) { for(j=0;j<1;j++) { ATL[i][j]=0; for(intk=0;k<8;k++) { ATL[i][j]+=A[k][i]*L[k][j]; } } }*/ //调用函数计算矩阵ATA的逆矩阵 //inverse(ATA); ContraryMatrix(*_ATA,*ATA,6); for(i=0;i<6;i++) { for(j=0;j<6;j++) { printf("%f",ATA[i][j]); if(j%5==0&&j! =0) { printf("\n"); } } } //计算矩阵X的各个元素值 for(i=0;i<6;i++) { X[i][0]=ATA[i][0]*ATL[0][0]+ATA[i][1]*ATL[1][0]+ATA[i][2]*ATL[2][0]+ATA[i][3]*ATL[3][0] +ATA[i][4]*ATL[4][0]+ATA[i][5]*ATL[5][0]; printf("%f",X[i][0]); } //for(i=0;i<6;i++) //{ //for(j=0;j<1;j++) //{ //X[i][j]=0; //for(intk=0;k<6;k++) //{ //X[i][j]+=ATA[i][k]*ATL[k][j]; // //} //printf("%f",X[i][j]); //} //} DXs=X[0][0]; DYs=X[1][0]; DZs=X[2][0]; Dphi=X[3][0]; Domega=X[4][0]; Dkappa=X[5][0]; Xs+=DXs;Ys+=DYs;Zs+=DZs;phi+=Dphi;omega+=Domega;kappa+=Dkappa; //计算矩阵V的各个元素 for(i=0;i { V[2*i][0]=A[2*i][0]*DXs+A[2*i][1]*DYs+A[2*i][2]*DZs+A[2*i][3]*Dphi+A[2*i][4]*Domega+A[2*i][5]*Dkappa-L[2*i][0]; V[2*i+1][0]=A[2*i+1][0]*DXs+A[2*i+1][1]*DYs+A[2*i+1][2]*DZs+A[2*i+1][3]*Dphi+A[2*i+1][4]*Domega+A[2*i+1][5]*Dkappa-L[2*i+1][0]; } /*//像点坐标改正后的值 for(i=0;i { coor[i].x+=V[2*i][0]; coor[i].y+=V[2*i+1][0]; }*/ } //判断限差的条件 while(! (fabs(Dphi)<0.1/60/180*PI&&fabs(Domega)<0.1/60/180*PI&&fabs(Dkappa)<0.1/60/180*PI)); //外方位元素计算完毕 //有关精度的计算 for(i=0;i<6;i++) Q[i][i]=ATA[i][i]; m0=sqrt((V[0][0]*V[0][0]+V[1][0]*V[1][0]+V[2][0]*V[2][0]+V[3][0]*V[3][0]+V[4][0]*V[4][0]+V[5][0]*V[5][0]+V[6][0]*V[6][0]+V[7][0]*V[7][0])/(2*n-6)); //计算各未知数的中误差 m1=sqrt(Q[0][0])*m0; m2=sqrt(Q[1][1])*m0; m3=sqrt(Q[2][2])*m0; m4=sqrt(Q[3][3])*m0; m5=sqrt(Q[4][4])*m0; m6=sqrt(Q[5][5])*m0; printf("改正后的相对坐标及其对应的地面点坐标(x,y,Xt,Yt,Zt): \n"); for(i=0;i { printf("%lf\t%lf\t%lf\t%lf\t%lf",(x)[i],(y)[i],coor[i].Xt,coor[i].Yt,coor[i].Zt); printf("\n"); } printf("旋转矩阵R: \n"); for(i=0;i<3;i++) { for(j=0;j<3;j++) printf("%lf\t",R[i][j]); printf("\n"); } printf("外方位元素(Xs,Ys,Zs,phi,omega,kappa)及改正值: \n"); printf("%lf+-%lf\n%lf+-%lf\n%lf+-%lf\n%lf+-%lf\n%lf+-%lf\n%lf+-%lf\n",Xs,DXs,Ys,DYs,Zs,DZs,phi,Dphi,omega,Domega,kappa,Dkappa); printf("单位权中误差: \n"); printf("%0.9f\n",m0); printf("外方位元素(Xs,Ys,Zs,phi,omega,kappa)的中误差: \n"); printf("%lf\n%lf\n%lf\n%lf\n%lf\n%lf\n",m1,m2,m3,m4,m5,m6); //计算完毕,输出结果,以文件方式保存 printf("计算结果存储在文件中\n"); FILE*fp; fp=fopen("计算结果.txt","w"); fprintf(fp,"改正后的相对坐标及其对应的地面点坐标(x,y,Xt,Yt,Zt): \n"); for(i=0;i { fprintf(fp,"%lf\t%lf\t%lf\t%lf\t%lf",coor[i].x,coor[i].y,coor[i].Xt,coor[i]
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 语言 空间 后方 交会 源代码