摄影测量单片空间后方交会程序《C++》

输入文件形式如下：C++源程序如下：#includeiostream#includefstream#includecmath#includestring#includeiomanipusingnamespacestd;constintn=6;voidinverse(doublec[n][n]);templatetypenameT1,typenameT2voidtranspose(T1*mat1,T2*mat2,inta,intb);templatetypenameT1,typenameT2voidmulti(T1*mat1,T2*mat2,T2*result,inta,intb,intc);templatetypenameTvoidinput(T*mat,inta,intb);templatetypenameTvoidoutput(T*mat,char*s,inta,intb);intmain(){ofstreamoutFile;cout.precision(5);doublex0=0.0,y0=0.0;doublefk=0.15324;//内方位元素doublem=39689;//估算比例尺doubleB[4][5]={0.0},R[3][3],XG[6][1],AT[6][8],ATA[6][6],ATL[6][1];input(B,4,5);//从文件中读取控制点的影像坐标和地面坐标，存入数组BdoubleXs=0.0,Ys=0.0,Zs=0.0,Q=0.0,W=0.0,K=0.0;doubleX,Y,Z,L[8][1],A[8][6];//确定未知数的出始值for(inti=0;i4;i++){Xs=Xs+B[i][2];Ys=Ys+B[i][3];Zs=Zs+B[i][4];}Xs=Xs/4;Ys=Ys/4;Zs=Zs/4+m*fk;intf=0;do//迭代计算{f++;//组成旋转矩阵R[0][0]=cos(Q)*cos(K)-sin(Q)*sin(W)*sin(K);R[0][1]=-cos(Q)*sin(K)-sin(Q)*sin(W)*cos(K);R[0][2]=-sin(Q)*cos(W);R[1][0]=cos(W)*sin(K);R[1][1]=cos(W)*cos(K);R[1][2]=-sin(W);R[2][0]=sin(Q)*cos(K)+cos(Q)*sin(W)*sin(K);R[2][1]=-sin(Q)*sin(K)+cos(Q)*sin(W)*cos(K);R[2][2]=cos(Q)*cos(W);//计算系数阵和常数项for(inti=0,k=0,j=0;i=3;i++,k++,j++){X=R[0][0]*(B[i][2]-Xs)+R[1][0]*(B[i][3]-Ys)+R[2][0]*(B[i][4]-Zs);Y=R[0][1]*(B[i][2]-Xs)+R[1][1]*(B[i][3]-Ys)+R[2][1]*(B[i][4]-Zs);Z=R[0][2]*(B[i][2]-Xs)+R[1][2]*(B[i][3]-Ys)+R[2][2]*(B[i][4]-Zs);L[j][0]=B[i][0]-(x0-fk*X/Z);L[j+1][0]=B[i][1]-(y0-fk*Y/Z);j++;A[k][0]=(R[0][0]*fk+R[0][2]*(B[i][0]-x0))/Z;A[k][1]=(R[1][0]*fk+R[1][2]*(B[i][0]-x0))/Z;A[k][2]=(R[2][0]*fk+R[2][2]*(B[i][0]-x0))/Z;A[k][3]=(B[i][1]-y0)*sin(W)-((B[i][0]-x0)*((B[i][0]-x0)*cos(K)-(B[i][1]-y0)*sin(K))/fk+fk*cos(K))*cos(W);A[k][4]=-fk*sin(K)-(B[i][0]-x0)*((B[i][0]-x0)*sin(K)+(B[i][1]-y0)*cos(K))/fk;A[k][5]=B[i][1]-y0;A[k+1][0]=(R[0][1]*fk+R[0][2]*(B[i][1]-y0))/Z;A[k+1][1]=(R[1][1]*fk+R[1][2]*(B[i][1]-y0))/Z;A[k+1][2]=(R[2][1]*fk+R[2][2]*(B[i][1]-y0))/Z;A[k+1][3]=-(B[i][0]-x0)*sin(W)-((B[i][1]-y0)*((B[i][0]-x0)*cos(K)-(B[i][1]-y0)*sin(K))/fk-fk*sin(K))*cos(W);A[k+1][4]=-fk*cos(K)-(B[i][1]-y0)*((B[i][0]-x0)*sin(K)+(B[i][1]-y0)*cos(K))/fk;A[k+1][5]=-(B[i][0]-x0);k++;}transpose(A,AT,6,8);multi(AT,A,ATA,6,8,6);inverse(ATA);multi(AT,L,ATL,6,8,1);multi(ATA,ATL,XG,6,6,1);Xs=Xs+XG[0][0];Ys=Ys+XG[1][0];Zs=Zs+XG[2][0];Q=Q+XG[3][0];W=W+XG[4][0];K=K+XG[5][0];}while(XG[3][0]=6.0/206265.0||XG[4][0]=6.0/206265.0||XG[5][0]=6.0/206265.0);cout迭代次数为：fendl;//精度评定doubleAXG[8][1],V[8][1],VT[1][8],VTV[1][1],m0,D[6][6];multi(A,XG,AXG,8,6,1);for(i=0;i8;i++)//计算改正数V[i][0]=AXG[i][0]-L[i][0];transpose(V,VT,1,8);multi(VT,V,VTV,1,8,1);m0=VTV[0][0]/2;for(i=0;i6;i++)for(intj=0;j6;j++)D[i][j]=m0*ATA[i][j];//屏幕输出误差方程系数阵、常数项、改正数output(A,误差方程系数阵A为：,8,6);output(L,常数项L为：,8,1);output(XG,改正数为：,6,1);outFile.open(aim.txt,ios::app);//打开并添加aim.txt文件outFile.precision(10);//以文件的形式输出像片外方位元素、旋转矩阵、方差阵outFile一、像片的外方位元素为：endlendl;outFilesetw(10)Xs=Xssetw(10)Ys=Yssetw(10)Zs=Zsendl;outFilesetw(20)航向倾角为：Qsetw(10)旁向倾角为：Wsetw(10)像片旋角为：Kendl;outFileendl二、旋转矩阵R为:endlendl;for(i=0;i3;i++){for(intj=0;j3;j++)outFilesetw(25)R[i][j]setw(25);outFileendl;}outFileendl;outFilesetw(0)三、精度评定结果为：endl;outFile.precision(5);for(i=0;i6;i++){for(intj=0;j6;j++)outFilesetw(14)D[i][j]setw(14);outFileendl;}outFile.close();return0;}templatetypenameT1,typenameT2voidtranspose(T1*mat1,T2*mat2,inta,intb){inti,j;for(i=0;ib;i++)for(j=0;ja;j++)mat2[j][i]=mat1[i][j];return;}templatetypenameT1,typenameT2voidmulti(T1*mat1,T2*mat2,T2*result,inta,intb,intc){inti,j,k;for(i=0;ia;i++){for(j=0;jc;j++){result[i][j]=0;for(k=0;kb;k++)result[i][j]+=mat1[i][k]*mat2[k][j];}}return;}templatetypenameTvoidinput(T*mat,inta,intb){ifstreaminFile;inFile.open(控制点坐标.txt);while(!inFile.eof()){for(inti=0;ia;i++)for(intj=0;jb;j++)inFilemat[i][j];}inFile.close();return;}templatetypenameTvoidoutput(T*mat,char*s,inta,intb){coutsetw(15)sendl;for(inti=0;ia;i++){for(intj=0;jb;j++)coutsetw(13)mat[i][j];coutendl;}return;}voidinverse(doublec[n][n]){inti,j,h,k;doublep;doubleq[n][12];for(i=0;in;i++)//构造高斯矩阵for(j=0;jn;j++)q[i][j]=c[i][j];for(i=0;in;i++)for(j=n;j12;j++){if(i+6==j)q[i][j]=1;elseq[i][j]=0;}for(h=k=0;kn-1;k++,h++)//消去对角线以下的数据for(i=k+1;in;i++){if(q[i][h]==0)continue;p=q[k][h]/q[i][h];for(j=0;j12;j++){q[i][j]*=p;q[i][j]-=q[k][j];}}for(h=k=n-1;k0;k--,h--)//消去对角线以上的数据for(i=k-1;i=0;i--){if(q[i][h]==0)continue;p=q[k][h]/q[i][h];for(j=0;j12;j++){q[i][j]*=p;q[i][j]-=q[k][j];}}for(i=0;in;i++)//将对角线上数据化为1{p=1.0/q[i][i];for(j=0;j12;j++)q[i][j]*=p;}for(i=0;in;i++)//提取逆矩阵for(j=0;jn;j++)c[i][j]=q[i][j+6];}程序的结果输出如下：（包括文本输出结果和荧屏输出中间数据）