攝影測量單片空間后方交會程序C

1、輸入文件形式如下：C+源程序如下：#include <iostream>#include <fstream>#include <cmath>#include <string>#include <iomanip>using namespace std;const int n=6;void inverse (double cnn);template<typename T1,typename T2>void transpose (T1*mat1,T2*mat2,int a,int b);template<typename

2、T1,typename T2>void multi(T1*mat1,T2 * mat2,T2 * result,int a,int b,int c);template<typename T>void input (T*mat,int a,int b);template<typename T>void output(T*mat,char*s,int a,int b);int main()ofstream outFile;cout.precision(5);double x0=0.0, y0=0.0; double fk=0.15324; /內方位元素double m

3、=39689; /估算比例尺 double B45=0.0,R33,XG61,AT68,ATA66,ATL61;input (B,4,5); /從文件中讀取控制點的影像坐標和地面坐標，存入數組Bdouble Xs=0.0, Ys=0.0, Zs=0.0,Q=0.0,W=0.0,K=0.0; double X,Y,Z,L81,A86; /確定未知數的出始值for(int i=0;i<4;i+)Xs=Xs+Bi2; Ys=Ys+Bi3; Zs=Zs+Bi4;Xs=Xs/4; Ys=Ys/4; Zs=Zs/4+m*fk;int f=0;do/迭代計算f+;/組成旋轉矩陣 R00=cos(Q)*

4、cos(K)-sin(Q)*sin(W)*sin(K); R01=-cos(Q)*sin(K)-sin(Q)*sin(W)*cos(K); R02=-sin(Q)*cos(W); R10=cos(W)*sin(K); R11=cos(W)*cos(K); R12=-sin(W); R20=sin(Q)*cos(K)+cos(Q)*sin(W)*sin(K); R21=-sin(Q)*sin(K)+cos(Q)*sin(W)*cos(K); R22=cos(Q)*cos(W); /計算系數陣和常數項 for(int i=0,k=0,j=0;i<=3;i+,k+,j+) X=R00*(Bi2

5、-Xs)+R10*(Bi3-Ys)+R20*(Bi4-Zs);Y=R01*(Bi2-Xs)+R11*(Bi3-Ys)+R21*(Bi4-Zs);Z=R02*(Bi2-Xs)+R12*(Bi3-Ys)+R22*(Bi4-Zs); Lj0=Bi0-(x0-fk*X/Z);Lj+10=Bi1-(y0-fk*Y/Z);j+;Ak0=(R00*fk+R02*(Bi0-x0)/Z;Ak1=(R10*fk+R12*(Bi0-x0)/Z;Ak2=(R20*fk+R22*(Bi0-x0)/Z;Ak3=(Bi1-y0)*sin(W)-(Bi0-x0)*(Bi0-x0)*cos(K)-(Bi1-y0)*sin(K)

6、/fk+fk*cos(K)*cos(W);Ak4=-fk*sin(K)-(Bi0-x0)*(Bi0-x0)*sin(K)+(Bi1-y0)*cos(K)/fk;Ak5=Bi1-y0;Ak+10=(R01*fk+R02*(Bi1-y0)/Z;Ak+11=(R11*fk+R12*(Bi1-y0)/Z;Ak+12=(R21*fk+R22*(Bi1-y0)/Z;Ak+13=-(Bi0-x0)*sin(W)-(Bi1-y0)*(Bi0-x0)*cos(K)-(Bi1-y0)*sin(K)/fk-fk*sin(K)*cos(W);Ak+14=-fk*cos(K)-(Bi1-y0)*(Bi0-x0)*sin

7、(K)+(Bi1-y0)*cos(K)/fk;Ak+15=-(Bi0-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+XG00; Ys=Ys+XG10; Zs=Zs+XG20;Q=Q+XG30; W=W+XG40; K=K+XG50;while(XG30>=6.0/206265.0|XG40>=6.0/206265.0|XG50>=6.0/206265.0);cout<<"迭代

8、次數為："<<f<<endl;/精度評定double AXG81,V81,VT18,VTV11,m0,D66;multi(A,XG,AXG,8,6,1); for( i=0;i<8;i+) /計算改正數Vi0=AXGi0-Li0; transpose (V,VT,1,8); multi(VT,V,VTV,1,8,1); m0=VTV00/2;for(i=0;i<6;i+)for(int j=0;j<6;j+)Dij=m0*ATAij;/屏幕輸出誤差方程系數陣、常數項、改正數output(A,"誤差方程系數陣A為：",8,6

9、);output(L,"常數項L為：",8,1);output(XG,"改正數為：",6,1);outFile.open("aim.txt",ios:app); /打開并添加aim.txt文件 outFile.precision(10);/以文件的形式輸出像片外方位元素、旋轉矩陣、方差陣outFile<<"一、像片的外方位元素為："<<endl<<endl;outFile<<setw(10)<<"Xs="<<Xs<&l

10、t;setw(10)<<"Ys="<<Ys<<setw(10)<<"Zs="<<Zs<<endl;outFile<<setw(20)<<"航向傾角為："<<Q<<setw(10)<<"旁向傾角為："<<W<<setw(10)<<"像片旋角為："<<K<<endl;outFile<<endl<

11、;<"二、旋轉矩陣R為:"<<endl<<endl;for( i=0;i<3;i+)for(int j=0;j<3;j+)outFile<<setw(25)<<Rij<<setw(25);outFile<<endl;outFile<<endl;outFile<<setw(0)<<"三、精度評定結果為："<<endl;outFile.precision(5);for(i=0;i<6;i+)for(int j=0;j

12、<6;j+)outFile<<setw(14)<<Dij<<setw(14);outFile<<endl;outFile.close();return 0;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+)mat2ji=mat1ij;return;template<typename T1,typename T2>void m

13、ulti(T1*mat1,T2 * mat2,T2 * result,int a,int b,int c) int i,j,k;for(i=0;i<a;i+)for(j=0;j<c;j+)resultij=0;for(k=0;k<b;k+)resultij+=mat1ik*mat2kj;return;template <typename T>void input (T*mat,int a,int b) ifstream inFile;inFile.open("控制點坐標.txt");while(!inFile.eof()for (int i=0

14、;i<a;i+) for(int j=0;j<b;j+)inFile>>matij;inFile.close();return;template<typename T>void output(T*mat,char*s,int a,int b) cout<<setw(15)<<s<<endl;for(int i=0;i<a;i+)for(int j=0;j<b;j+) cout<<setw(13)<<matij;cout<<endl;return;void inverse(do

15、uble cnn) int i,j,h,k; double p; double qn12; for(i=0;i<n;i+)/構造高斯矩陣 for(j=0;j<n;j+) qij=cij; for(i=0;i<n;i+) for(j=n;j<12;j+) if(i+6=j) qij=1; else qij=0; for(h=k=0;k<n-1;k+,h+)/消去對角線以下的數據 for(i=k+1;i<n;i+) if(qih=0) continue; p=qkh/qih; for(j=0;j<12;j+) qij*=p; qij-=qkj; for(h=k=n-1;k>0;k-,h-) / 消去對角線以上的數據 for(i