班级：测绘一班 学号:20133279日期：2015426Southw^ JIaotong UnjiverSity、计算原理.二、算法流程.三、源程序.四、计算结果.五、结果分析.六、心得体会. 目录131313、计算原理已知条件摄影机主距f=153.24mm, xO=O, yO=O,像片比例尺为1:40000,有四对点的像点坐标与相应的地面坐标如下表。

二、算法流程（1）获取已知数据。

从航摄资料中差取平均航高与摄影机主距；获取控制点的地面测量坐标并转换为地面摄影坐标。

（2）量测控制点的像点坐标并作系统误差改正。

（3）确定未知数的初始值。

在竖直摄影且地面控制点大体对称分布的情况下，按如下方法确定初始值，即x S 上，Y s0n丄Z0n，Z s mf（4）用三个角元素的初始值按下式，计算各个方向余弦值，组成旋转矩阵R31 cos cos sin sin sina2cos sin sin sin cos33 sin cosb1 cos sinb2 cos cosb3 sinC1 sin cos cos sin sin'c sin sin cos sin cosC3 cos cos（5）逐点计算像点坐标的近似值。

禾I」用未知数的近似值和控制点的地面坐标; 带入共线方程式，逐点近似像点坐标的近似值（（6）(7) X）、（y）。

逐点计算误差方程式的系数和常数项，组成误差方程式。

计算法方程的系数矩阵A A和常数项A L L，组成法方程式。

(8) 解法方程，求得外方位元素的改正数dX s、dY s、dZ s、d、d、d 。

(9)用前次迭代取得的近似值，加本次迭代的改正数,xr xL dX S\Y S K Y S K1 dY S K,Z K 计算外方位元素的新值。

z K1dz（K K 1 . K K K 1 . K Kd , d ,（10）将求得的外方位元素改正数与规定的限差比较，负责用新的近似值重复（4）-（9），直到满足要求为止。

若小于限差，则迭代结束。

用共线方程进行空间后方交会的程序框如图所示。

输入原始数据像点坐标计算，系统误差正 确定外方位因素初始值组成旋转矩阵R所有像点完否是C解法方程，求外方位元素改正数计算改正后的外方位元素 外方位元素改正数是否小于限差 输出计算成果，计算并结束、源程序#in elude ”ostream" using n ames pace std;#in elude "fstream" #in clude <ioma nip>const int n=6;void in verse(double c[n ][ n]);temp late<t ypen ame Tl,t ypen ame T2>void transp ose (TI*mat1,T2*mat2,i nt a,i nt b);temp latevty pen ame TI,t ypen ame T2>void multi(T1*mat1,T2*mat2,T2*result, i nt a, i nt b, i nt c); int mai n() {doublex[4][2]={-0.08615,-0.06899,-0.05340,0.08221 ，-0.01478，-0.07663,0.01046,0.06443};doubleX[4][3]={36589.41,25273.32,2195.17,37631.08,31324.51,728.69,39100.97,24934.98, 2386.50,40426.54,30319.81,757.31};int i,j,num=0;//num 为迭代次数double X0[6]={0};//设定未知数(XS,YS,ZS,三个角度)初始值 double f=0.15324;// 摄影机主距 f=153.24mm结束并显示错误信息逐点组成误差方程式并法化否否L迭代次数小于ndouble a=1/40000.0;// 像片比例尺为 1:40000 double R[3][3]={0};// 初始化旋转知阵 R double approx_ x[8]={0};// 用于存放像点估计值 double A[8][6] 二 {0};// 定义了一个系数阵 double AT[6][8] 二 {0};// 定义了 A 的转置知阵 double L[8]={0}:// 定义常数项 const double pi=3.1415926535897932; double Asum[6][6]={0}; double result2[6]={0}; double resultl[6][8]={0}; double sumXYZ[3]={0}; cout.precision(5); cout<< ”已知像点坐标为 :\n ”; for(i=0;i<4;i++)for(j=0;j<2;j++){cout<<fixed: if (j==0)cout<< ”x ”<<i+l<< ”=”<<setw (8)<<x[i][j]<<{cout<< ”Z ”;cout<<i+1;cout<<}}cout<<endl; for(j=0;j<3;j++)for(i=0;i<4;i++) sumXYZ[j]+=X[i][j];for(i=0;i<2;i++)X0[i]=sumXYZ[i]/4;//X0,Y0 初始化X0[i]=1/a*f+sumXYZ[2]/4. 0;// 对 Z0 进行初始化 do{}else{cout<< ”y"<<i+1<< ”=”<<setw(6)<<x[i][j]<<en dl;}}cout<< ”己知地面四个点的坐标为 for(i=0;i<4;i++) for(j=0;j<3;j++) {if (j==0){cout<< ”X";cout<<i+1;cout<<}else if(j==1) {cout<< ”Y";cout<<i+1;cout<<}else:\n ”;<<X[i][j]<< ” ”<<X[i][j)<< ” ””;cout<<X[i][j]<<endl;R[0][0]=cos(X0[3])*cos(X0[5])-sin(X0[3])*sin(X0[4])*sin(X0[5]);R[0][I]=-cos(X0[3])*sin(X0[5])-sin(X0[3])*sin(X0[4])*cos(X0[5]);R[0][2]=-sin(X0[3])*cos(X0[4]);R[1][0]=cos(X0[4])*sin(X0[5]);R[I1[1]=cos(X0[4])*cos(X0[5]);R[1][2]=-sin(X0[4]);R[2][0]=sin(X0[3])*cos(X0[5])+cos(X0[3])*sin(X0[4])*sin(X0[5]);R[2][l]=-sin(X0[3])*sin(X0[5])+cos(X0[3])*sin(X0[4])*cos(X0[5]);R[2][2]=cos(X0[3])*cos(X0[4]);//第一个像点的估计值，第一个点的坐标存放于X [0] [0],X [0] [1],X [0] [2]approx_x[0]=-f*(R[0][0]*(X[0][0]-X0[0])+R[1][0]*(X[0][1]-X0[1])+R[2][0]*(X[0][2]-X0[2]))/( R[0][2]*(X[0][0]-X0[0])+R[1][2]*(X[0][1]-X0[1])+R[2][2]*(X[0][2]-X0[2])); approx_x[1]=-f*(R[0][1]*(X[0][0]-X0[0])+R[1][1]*(X[0][1]-X0[1])+R[2][1]*(X[0][2]-X0[2]))/( R[0][2]*(X[0][0]-X0[0])+R[1][2]*(X[0][1]-X0[1])+R[2][2]*(X[0][2]-X0[2])); //第二个像点的估计值,第 2 个点的坐标存放于X[1][0],X[1][1],X[1][2] approx_x[2]=-f*(R[0][0]*(X[1][0]-X0[0])+R[1][0]*(X[1][1]-X0[1])+R[2][0]*(X[1][2]-X0[2]))/( R[0][2]*(X[1][0]-X0[0])+R[1][2]*(X[1][1]-X0[1])+R[2][2]*(X[1][2]-X0[2])); approx_x[3]=-f*(R[0][1]*(X[1][0]-X0[0])+R[1][1]*(X[1][1]-X0[1])+R[2][1]*(X[1][2]-X0[2]))/( R[0][2]*(X[1][0]-X0[0])+R[1][2]*(X[1][1]-X0[1])+R[2][2]*(X[1][2]-X0[2]));//第三个像点的估计值,第 3 个点的坐标存放于X[2][0],X[2][1],X[2][2] approx_x[4]=-f*(R[0][0]*(X[2][0]-X0[0])+R[1][0]*(X[2][1]-X0[1])+R[2][0]*(X[2][2]-X0[2]))/( R[0][2]*(X[2][0]-X0[0])+R[1][2]*(X[2][1]-X0[1])+R[2][2]*(X[2][2]-X0[2])); approx_x[5]=-f*(R[0][1]*(X[2][0]-X0[0])+R[1][1]*(X[2][1]-X0[1])+R[2][1]*(X[2][2]-X0[2]))/( R[0][2]*(X[2][0]-X0[0])+R[1][2]*(X[2][1]-X0[1])+R[2][2]*(X[2][2]-X0[2])); //第四个像点的估计值,第 4 个点的坐标存放于X[3][0],X[3][1],X[3][2] approx_x[6]=-f*(R[0][0]*(X[3][0]-X0[0])+R[1][0]*(X[3][1]-X0[1])+R[2][0]*(X[3][2]-X0[2]))/( R[0][2]*(X[3][0]-X0[0])+R[1][2]*(X[3][1]-X0[1])+R[2][2]*(X[3][2]-X0[2])); approx_x[7]=-f*(R[0][1]*(X[3][0]-X0[0])+R[1][1]*(X[3][1]-X0[1])+R[2][1]*(X[3][2]-X0[2]))/( R[0][2]*(X[3][0]-X0[0])+R[1][2]*(X[3][1]-X0[1])+R[2][2]*(X[3][2]-X0[2])); for(i=0;i<4;i++){//第i 个像点估计值放在approx_x[2*(i-1)],approx_x[2*i-1]/*a11*/A[2*i][0]=(R[0][0]*f+R[0][2]*approx_x[2*i])/(R[0][2]*(X[i][0]-X0[0])+R[1][2]*(X[i][1]-X0[1])+R[2][2]*(X[i][2]-X0[2]));/*a12*/A[2*i][1]=(R[1][0]*f+R[1][2]*approx_x[2*i])/(R[0][2]*(X[i][0]-X0[0])+R[1][2]*(X[i][1] -X0[1])+R[2][2]*(X[i][2]-X0[2]));/*a13*/A[2*i][2]=(R[2][0]*f+R[2][2]*approx_x[2*i])/(R[0][2]*(X[i][0]-X0[0])+R[1][2]*(X[i][1] -X0[1])+R[2][2]*(X[i][2]-X0[2]));/*a21*/A[2*i+1][0]=(R[0][1]*f+R[0][2]*approx_x[2*i+1])/(R[0][2]*(X[i][0]-X0[0])+R[1][2]*(X[i][1]-X0[1])+R[2][2]*(X[i][2]-X0[2]));/*a22*/A[2*i+1][1]=(R[1][1]*f+R[1][2]*approx_x[2*i+1])/(R[0][2]*(X[i][0]-X0[0])+R[1][2]*(X[i][1]-X0[1])+R[2][2]*(X[i][2]-X0[2]));/*a23*/A[2*i+1][2]=(R[2][1]*f+R[2][2]*approx_x[2*i+1])/(R[0][2]*(X[i][0]-X0[0])+R[1][2]*(X[i][1]-X0[1])+R[2][2]*(X[i][2]-X0[2]));/*a14*/A[2*i][3]=approx_x[2*i+1]*sin(X0[4])-(approx_x[2*i]/f*(approx_x[2*i]*cos(X0[5])-approx_x[2*i+1]*sin(X0[5]))+f*cos(X0[5]))*cos(X0[4]);/*a15*/A[2*i][4]=-f*sin(X0[5])-approx_x[2*i]/f*(approx_x[2*i]*sin(X0[5])+approx_x[2*i+1]*co s(X0[5])); /*a16*/A[2*i][5]=approx_x[2*i+1];/*a24*/A[2*i+1][3]=-1*approx_x[2*i]*sin(X0[4])-(approx_x[2*i+1]/f*(approx_x[2*i]*cos(X0[5] )-approx_x[2*i+1]*sin(X0[5]))-f*sin(X0[5]))*cos(X0[4]);/*a25*/A[2*i+1][4]=-1*f*cos(X0[5])-approx_x[2*i+1]/f*(approx_x[2*i]*sin(X0[5])+approx_x[2*i+1]*cos(X0[5]));/*a26*/A[2*i+1][5]=-approx_x[2*i];}//进行常数项的初始化for(i=0;i<4;i++){L[2*i]=x[i][0]-approx_x[2*i];L[2*i+1]=x[i][1]-approx_x[2*i+1];}//A 的转置矩阵for(i=0;i<8;i++)for(j=0;j<6;j++){AT[j][i]=A[i][j];}// 实现 A 与AT 相乘int k=0;for(i=0;i<6;i++)for(j=0;j<6;j++)Asum[i][j]=0;for(i=0;i<6;i++)for(k=0;k<6;k++)for(j=0;j<8;j++)Asum[i][k]+=A T[i][j]*A[j][k];// 得到AT*A 的逆矩阵存放在inverseAsum[6][6] 中inverse(Asum);//实现矩阵Asum[6][6] 与AT[6][8] 的相乘,结果存放在result1[6][8] 中for(i=0;i<6;i++)for(j=0;j<8;j++)result1[i][j]=0;for(i=0;i<6;i++) for(k=0;k<8;k++)for(j=0;j<6;j++)result1[i][k]+=Asum[i][j]*AT[j][k];//实现resultl [6][8]与1[8]的相乘，得到结果放在result2[6]中;for(i=0;i<6;i++)result2[i]=0;for(i=0;i<6;i++)for(j=0;j<8;j++)result2[i]+=result1[i][j]*L[j];num++;for(i=0;i<6;i++){X0[i]=X0[i]+result2[i];}ofstream f7("d:\\A.txt");f7<< std::fixed;cout<<"进行第"<<num<<"次迭代带得到Xs,Ys,Zs, ^,3, K改正数分别为：\n";for(i=0;i<6;i++){ cout<<setw(12)<<result2[i];f7<<setw(12)<<result2[i];}cout<<endl<<endl;f7.close();getchar();}while(abs(result2[3]*206265.0)>6||abs(result2[4]*206265.0)>6||abs(result2[5]*20626 5.0)>6);cout<<"\n 满足限差条件结束循环，最终结果为：\n";cout<<setw(12)<<"Xs"<<setw(12)<<"Ys"<<setw(12)<<"Zs"<<setw(12)<<" p "<<setw(12)<<" 3"<<setw(12)<<" K "<<endl;ofstream f7("d:\\A.txt");f7<< std::fixed;cout.precision(4);for(i=0;i<6;i++){cout<<setw(12)<<X0[i];f7<<setw(16)<<X0[i];}f7.close();//今double XG[6][1];for(i=0;i<6;i++)XG[i][0]=result2[i];double AXG[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;i<8;i++)//计算改正数V[i][0]=AXG[i][0]-L[i];transpose (V,VT,1,8);multi(VT,V ,VTV,1,8,1);m0=VTV[0][0]/2;cout<<endl;ofstream f6("d:\\what.txt");// f6<< std::fixed;for(i=0;i<6;i++){for(int j=0;j<6;j++){D[i][j]=m0*Asum[i][j]; cout<<setw(10)<<D[i][j];f6<<setw(15)<<D[i][j];}cout<<endl;f6<<endl;}for(i=0;i<6;i++)cout<<sqrt(D[i][i])<<endl;f6.close();//屏幕输出误差方程系数阵、常数项、改正数//getchar();return 0;}void inverse(double c[n][n]){ int i,j,h,k; double p;double q[n][12];for(i=0;i<n;i++)// 构造高斯矩阵for(j=0;j<n;j++)q[i][j]=c[i][j];for(i=0;i<n;i++)for(j=n;j<12;j++){if(i+6==j)q[i][j]=1;elseq[i][j]=0;}for(h=k=0;k<n-1;k++,h++)// 消去对角线以下的数据for(i=k+1;i<n;i++){ if(q[i][h]==0) continue; p=q[k][h]/q[i][h];for(j=0;j<12;j++){q[i][j]*=p;q[i][j]-=q[k][j];}}for(h=k=n-1;k>0;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;j<12;j++){q[i][j]*=p; q[i][j]-=q[k][j];}}for(i=0;i<n;i++)// 将对角线上数据化为 1{ p=1.0/q[i][i];for(j=0;j<12;j++)q[i][j]*=p;}for(i=0;i<n;i++) // 提取逆矩阵for(j=0;j<n;j++)c[i][j]=q[i][j+6];}template<typename T1,typename T2>void transpose(T1*mat1,T2*mat2,int a,int b){ int i,j;for(i=0;i<b;i++)for(j=0;j<a;j++)mat2[j][i]=mat1[i][j];return;}template<typename T1,typename T2>void multi(T1*mat1,T2 * mat2,T2 * result,int a,int b,int c){for(i=0;i<a;i++) for(j=0;j<c;j++) result[i][j]=0; for(k=0;k<b;k++) result[i][j]+=mat1[i][k]*mat2[k][j];return;程序截图五、结果分析六、心得体会本次实验是关于单张像片的空间后方交会的计算原理及实现过程的课程作业， 其中解题步骤和公式是基本， 熟练运用公式并且成功编程是重点。