攝影測量程序匯總后方交會前方交會單模型光束法平差

1、程序運行環(huán)境為Visual Studio2010.運行前請先將坐標(biāo)數(shù)據(jù)放在debug下。1. 單像空間后方交會原始數(shù)據(jù)：內(nèi)方位元素x0(/mm)y0(/mm)主距f(/mm)比例尺分母00153.2450000像點坐標(biāo)(/mm)物點坐標(biāo)(/m)-86.15-68.9936589.4125273.322195.17-53.482.2137631.0831324.51728.69-14.78-76.6339100.9724934.982386.510.4664.4340426.5430319.81757.31C語言程序：#include #include #include double *read

2、data();void savedata(int hang,double *data,double *xishuarray,double *faxishu,double *l,int i,double xs,double ys,double zs,double fai,double oumiga,double kapa);void transpose(double *m1,double *m2,int m,int n);void inverse(double *a,int n);void multi(double *mat1,double * mat2,double * result,int

3、a,int b,int c);void inverse(double *a,int n)/*正定矩陣求逆*/ int i,j,k;for(k=0;kn;k+)for(i=0;in;i+)if(i!=k)*(a+i*n+k)=-*(a+i*n+k)/(*(a+k*n+k);*(a+k*n+k)=1/(*(a+k*n+k);for(i=0;in;i+)if(i!=k)for(j=0;jn;j+)if(j!=k)*(a+i*n+j)+=*(a+k*n+j)* *(a+i*n+k);for(j=0;jn;j+)if(j!=k)*(a+k*n+j)*=*(a+k*n+k);void transpose(

4、double *m1,double *m2,int m,int n) /矩陣轉(zhuǎn)置 int i,j;for(i=0;im;i+)for(j=0;jn;j+)m2j*m+i=m1i*n+j;return;void multi(double *mat1,double *mat2,double * result,int a,int b,int c) int i,j,k; for(i=0;ia;i+) for(j=0;jc;j+) resulti*c+j=0; for(k=0;kb;k+) resulti*c+j+=mat1i*b+k*mat2k*c+j; return;double *readdata

5、()FILE *fp;int i,j;int number;char datacatolog100;/scanf(%s,datacatolog);if (fp=fopen(控制點坐標(biāo).txt,r)=NULL)printf(讀取數(shù)據(jù)出錯!n);return false; fscanf(fp,%d,&number);double *cordata=new doublenumber*5;for (i=0;inumber;i+)for (j=0;j5;j+) fscanf(fp,%lf,cordata+i*5+j);printf(控制點坐標(biāo)數(shù)據(jù)讀取成功！n);return cordata;void s

6、avedata(int hang,double *data,double *xishuarray,double *faxishu,double *l,int i,double xs,double ys,double zs,double fai,double oumiga,double kapa)FILE *fp;char *file1=結(jié)算數(shù)據(jù).txt;fp=fopen(file1,w);fprintf(fp,-原始坐標(biāo)數(shù)據(jù)為-：n);for (int i=0;ihang;i+)for (int j=0;j5;j+)fprintf(fp,%7.4lf ,datai*5+j);fprintf(f

7、p,n);fprintf(fp,-n);fprintf(fp,-誤差方程系數(shù)陣為：-：n);for (int i=0;ihang*2;i+)for (int j=0;j6;j+)fprintf(fp,%7.4lf ,xishuarrayi*5+j);fprintf(fp,n);fprintf(fp,-n);fprintf(fp,-法方程系數(shù)陣為：-：n);for (int i=0;i6;i+)for (int j=0;j6;j+)fprintf(fp,%7.5lf ,faxishui*5+j);fprintf(fp,n);fprintf(fp,-n);fprintf(fp,-誤差方程常數(shù)項為:

8、-：n);for (int i=0;ihang*2;i+)fprintf(fp,%lf ,li);fprintf(fp,n);fprintf(fp,-n);fprintf(fp,-迭代次數(shù)為:-：n);fprintf(fp,%dn,i);fprintf(fp,-n);fprintf(fp,-外方位元素為：-n);fprintf(fp, Xs= %lf, Ys=%lf, Zs=%lfn,xs,ys,zs);fprintf(fp, fai= %lf, oumiga=%lf, kapa=%lfn,fai,oumiga,kapa);fprintf(fp,-n);fclose(fp);return;vo

9、id main()int i,j;int ii,jj;int diedainumber=0;double x0=0.0,y0=0.0,f=0.0;double m=50000; /估算比例尺double fai=0,oumiga=0,kapa=0,Xs=0,Ys=0,Zs=0;double R33=0.0;double X=0.0,Y=0.0,Z=0.0,L81=0.0,A86=0.0; double correct61=0.0,AT68=0.0,ATA66=0.0,ATL61=0.0;int row; /row用于存放坐標(biāo)行數(shù)double *controlpoint;controlpoint

10、=readdata();row=sizeof(controlpoint);for (i=0;irow;i+)for (j=0;j5;j+)printf(%3.3lf ,controlpointi*5+j);printf(n);/*-內(nèi)方位元素-*/printf(請輸入像片的內(nèi)方位元素(mm)：n);printf(x0=);x0/=1000.0;scanf(%lf,&x0); /double類型數(shù)據(jù)要用%lfprintf(y0=);y0/=1000.0;scanf(%lf,&y0);printf(f=);scanf(%lf,&f);f=f/1000.0;/*-*/-確定未知數(shù)初始值-for(in

11、t i=0;irow;i+)Xs=Xs+controlpointi*5+2; Ys=Ys+controlpointi*5+3; Zs=Zs+controlpointi*5+4;Xs/=row;Ys/=row;Zs=Zs/row+m*f;/- do diedainumber+;/-組成旋轉(zhuǎn)矩陣-R00=cos(fai)*cos(kapa)-sin(fai)*sin(oumiga)*sin(kapa);R01=-cos(fai)*sin(kapa)-sin(fai)*sin(oumiga)*cos(kapa);R02=-sin(fai)*cos(oumiga);R10=cos(oumiga)*si

12、n(kapa);R11=cos(oumiga)*cos(kapa);R12=-sin(oumiga);R20=sin(fai)*cos(kapa)+cos(fai)*sin(oumiga)*sin(kapa);R21=-sin(fai)*sin(kapa)+cos(fai)*sin(oumiga)*cos(kapa);R22=cos(fai)*cos(oumiga);/-/計算系數(shù)陣和常數(shù)項for(int i=0,k=0,j=0;i=6.0/206265.0|correct40=6.0/206265.0|correct50=6.0/206265.0); printf(迭代次數(shù)為：%dn,die

13、dainumber); printf(-誤差方程系數(shù)為：-n);for (i=0;i8;i+)for (j=0;j6;j+)printf(%4.4lf ,Aij);printf(n);printf(-n); printf(求解得到的外方位元素為：n);printf( Xs= %lfn,Xs);printf( Ys= %lfn,Ys);printf( Zs= %lfn,Zs);printf( fai= %lfn,fai);printf( oumiga= %lfn,oumiga);printf( kapa= %lfn,kapa);savedata(row,controlpoint,A0,ATA0,

14、L0,diedainumber,Xs,Ys,Zs,fai,oumiga,kapa);printf(-解算結(jié)束！-n);system(pause);解算結(jié)果：2. 后方交會-前方交會求解地面點坐標(biāo)已知左右像片外方位元素，給出像點坐標(biāo)：左像點坐標(biāo)：右像點坐標(biāo)：x（/m）y（/m）x（/m）y（/m）0.00530.00690.004820.00270.005560.002290.00521-0.001910.008150.004990.007730.00089-0.002470.00156-0.00279-0.00292C語言代碼：#include #include #include double

15、 *readdata();void savedata(int hang,double *data);double *readdata()FILE *fp;int i,j,k;int number;char datacatolog100;char leftdata300;/scanf(%s,datacatolog);if (fp=fopen(像點坐標(biāo)數(shù)據(jù).txt,r)=NULL)printf(讀取數(shù)據(jù)出錯!n);system(pause);exit(0);fscanf(fp,%d,&number);double *c=new doublenumber*4;for (k=0;k2;k+)fread

16、(&leftdata,14,1,fp);for (i=0;inumber;i+) for (j=0;j2;j+)fscanf(fp,%lf,c+k*2+i*4+j);fclose(fp);return c;void savedata(int hang,double *data)FILE *fp;char *file1=地面點坐標(biāo)數(shù)據(jù).txt;fp=fopen(file1,w);fprintf(fp,-像點對應(yīng)地面點坐標(biāo)為-：n);fprintf(fp,n);for (int i=0;ihang;i+)fprintf(fp,第%d點： ,i+1);for (int j=0;j3;j+)fprin

17、tf(fp,%7.4lf ,datai*3+j);fprintf(fp,nn);fprintf(fp,-);fclose(fp);return;void main()double *imagepoint;int row;int i,j;imagepoint=readdata();row=sizeof(imagepoint);/-double f=24;f/=1000;double fai1=-0.0061,oumiga1=0.0327,kapa1=0.1711,Ys1=397367.171,Xs1=3445820.098,Zs1=1486.212;double fai2=0.0063,oumi

18、ga2=0.0178,kapa2=0.1489,Ys2=397367.234,Xs2=3445959.266,Zs2=1490.096;/ printf(請輸入左像片的外方位元素：n);/printf(Xs1= );/scanf(%lf,&Xs1);/printf(Ys1= );/scanf(%lf,&Ys1);/printf(Zs1= );/scanf(%lf,&Zs1);/printf(fai1= );/scanf(%lf,&fai1);/printf(oumiga1= );/scanf(%lf,&oumiga1);/printf(kapa1= );/scanf(%lf,&kapa1);/

19、printf(請輸入右像片的外方位元素：n);/printf(Xs2= );/scanf(%lf,&Xs2);/printf(Ys2= );/scanf(%lf,&Ys2);/printf(Zs2= );/scanf(%lf,&Zs2);/printf(fai2= );/scanf(%lf,&fai2);/printf(oumiga2= );/scanf(%lf,&oumiga2);/printf(kapa2= );/scanf(%lf,&kapa2);double Bx=Xs2-Xs1,By=Ys2-Ys1,Bz=Zs2-Zs1;double N1=0,N2=0;double X1=0,Y1

20、=0,Z1=0,X2=0,Y2=0,Z2=0;double R133=0.0;double R233=0.0;double GEOdata43=0.0;for (i=0;irow;i+)/-組成左影像旋轉(zhuǎn)矩陣-R100=cos(fai1)*cos(kapa1)-sin(fai1)*sin(oumiga1)*sin(kapa1);R101=-cos(fai1)*sin(kapa1)-sin(fai1)*sin(oumiga1)*cos(kapa1);R102=-sin(fai1)*cos(oumiga1);R110=cos(oumiga1)*sin(kapa1);R111=cos(oumiga1

21、)*cos(kapa1);R112=-sin(oumiga1);R120=sin(fai1)*cos(kapa1)+cos(fai1)*sin(oumiga1)*sin(kapa1);R121=-sin(fai1)*sin(kapa1)+cos(fai1)*sin(oumiga1)*cos(kapa1);R122=cos(fai1)*cos(oumiga1);/-/-組成右影像旋轉(zhuǎn)矩陣-R200=cos(fai2)*cos(kapa2)-sin(fai2)*sin(oumiga2)*sin(kapa2);R201=-cos(fai2)*sin(kapa2)-sin(fai2)*sin(oumi

22、ga2)*cos(kapa2);R202=-sin(fai2)*cos(oumiga2);R210=cos(oumiga2)*sin(kapa2);R211=cos(oumiga2)*cos(kapa2);R212=-sin(oumiga2);R220=sin(fai2)*cos(kapa2)+cos(fai2)*sin(oumiga2)*sin(kapa2);R221=-sin(fai2)*sin(kapa2)+cos(fai2)*sin(oumiga2)*cos(kapa2);R222=cos(fai2)*cos(oumiga2);/-像空輔系坐標(biāo)-X1=R100*imagepointi*

23、4+0+R101*imagepointi*4+1-R102*f;Y1=R110*imagepointi*4+0+R111*imagepointi*4+1-R112*f;Z1=R120*imagepointi*4+0+R121*imagepointi*4+1-R122*f;X2=R200*imagepointi*4+2+R201*imagepointi*4+3-R202*f;Y2=R210*imagepointi*4+2+R211*imagepointi*4+3-R212*f;Z2=R220*imagepointi*4+2+R221*imagepointi*4+3-R222*f;/-/-點投影系

24、數(shù)-N1=(Bx*Z2-Bz*X2)/(X1*Z2-Z1*X2);N2=(Bx*Z1-Bz*X1)/(X1*Z2-Z1*X2);/-/-計算地面點坐標(biāo)-GEOdatai0=Xs1+N1*X1;GEOdatai1=Ys1+By+N2*Y2;GEOdatai2=Zs1+N1*Z1;/-/-for (i=0;i4;i+)printf(第%d個地面點坐標(biāo)： ,i+1);for (j=0;j3;j+)printf(%lf ,GEOdataij);printf(nn);savedata(row,GEOdata0);system(pause);測試結(jié)果：3. 單模型光束法嚴(yán)密平差缺少已知數(shù)據(jù)進行驗證，因此如

25、果有已知數(shù)據(jù)請代入已知數(shù)據(jù)進行驗證。C語言代碼：#include #include double *readdata();void savedata(int hang,double *data,double *xishuarray,double *faxishu,double *l,int i,double xs,double ys,double zs,double fai,double oumiga,double kapa,double xs2,double ys2,double zs2,double fai2,double oumiga2,double kapa2);void transp

26、ose(double *m1,double *m2,int m,int n);void inverse(double *a,int n);void multi(double *mat1,double * mat2,double * result,int a,int b,int c);void inverse(double *a,int n)/*正定矩陣求逆*/ int i,j,k;for(k=0;kn;k+)for(i=0;in;i+)if(i!=k)*(a+i*n+k)=-*(a+i*n+k)/(*(a+k*n+k);*(a+k*n+k)=1/(*(a+k*n+k);for(i=0;in;i

27、+)if(i!=k)for(j=0;jn;j+)if(j!=k)*(a+i*n+j)+=*(a+k*n+j)* *(a+i*n+k);for(j=0;jn;j+)if(j!=k)*(a+k*n+j)*=*(a+k*n+k);void transpose(double *m1,double *m2,int m,int n) /矩陣轉(zhuǎn)置 int i,j;for(i=0;im;i+)for(j=0;jn;j+)m2j*m+i=m1i*n+j;return;void multi(double *mat1,double *mat2,double * result,int a,int b,int c) i

28、nt i,j,k;for(i=0;ia;i+)for(j=0;jc;j+)resulti*c+j=0;for(k=0;kb;k+)resulti*c+j+=mat1i*b+k*mat2k*c+j;return;double *readdata()FILE *fp;int i,j,k;int number;char datacatolog100;char leftdata300;/scanf(%s,datacatolog);if (fp=fopen(控制點坐標(biāo).txt,r)=NULL)printf(讀取數(shù)據(jù)出錯!n);return false;fscanf(fp,%d,&number);doub

29、le *c=new doublenumber*7;for (k=0;k3;k+)fread(&leftdata,14,1,fp);for (i=0;i4;i+) if (k=2)for (j=0;j3;j+)fscanf(fp,%lf,c+k*2+i*7+j);elsefor (j=0;j2;j+)fscanf(fp,%lf,c+k*2+i*7+j);fclose(fp);return c;void savedata(int hang,double *data,double *xishuarray,double *faxishu,double *l,int i,double xs,double

30、 ys,double zs,double fai,double oumiga,double kapa,double xs2,double ys2,double zs2,double fai2,double oumiga2,double kapa2)FILE *fp;char *file1=結(jié)算數(shù)據(jù).txt;fp=fopen(file1,w);fprintf(fp,-原始坐標(biāo)數(shù)據(jù)為-：n);for (int i=0;ihang;i+)for (int j=0;j7;j+)fprintf(fp,%7.4lf ,datai*7+j);fprintf(fp,n);fprintf(fp,-n);fpri

31、ntf(fp,-誤差方程系數(shù)陣為：-：n);for (int i=0;ihang*4;i+)for (int j=0;j12;j+)fprintf(fp,%4.3e ,xishuarrayi*12+j);fprintf(fp,n);fprintf(fp,-n);fprintf(fp,-法方程系數(shù)陣為：-：n);for (int i=0;i12;i+)for (int j=0;j12;j+)fprintf(fp,%4.3e ,faxishui*12+j);fprintf(fp,n);fprintf(fp,-n);fprintf(fp,-誤差方程常數(shù)項為:-：n);for (int i=0;iha

32、ng*4;i+)fprintf(fp,%lf ,li);fprintf(fp,n);fprintf(fp,-n);fprintf(fp,-迭代次數(shù)為:-：n);fprintf(fp,%dn,i);fprintf(fp,-n);fprintf(fp,-左像的外方位元素為：-n);fprintf(fp, Xs1= %lf, Ys1=%lf, Zs1=%lfn,xs,ys,zs);fprintf(fp, fai1= %lf, oumiga1=%lf, kapa1=%lfn,fai,oumiga,kapa);fprintf(fp,-n);fprintf(fp,-右像的外方位元素為：-n);fprint

33、f(fp, Xs2= %lf, Ys2=%lf, Zs2=%lfn,xs2,ys2,zs2);fprintf(fp, fai2= %lf, oumiga2=%lf, kapa2=%lfn,fai2,oumiga2,kapa2);fprintf(fp,-n);fclose(fp);return;void main()double *controlpoint;int row;int i,j;controlpoint=readdata();row=sizeof(controlpoint);double f=0.024;double x0left=0,y0left=0;double x0right=0

34、,y0right=0;double fai1=0,oumiga1=0,kapa1=0,Ys1=397510,Xs1=3445853,Zs1=1455;double fai2=0,oumiga2=0,kapa2=0,Ys2=397513,Xs2=3445979,Zs2=1453.685;double H=830; printf(請輸入攝影機焦距：);scanf(%f=lf,&f);f=f/1000;printf(請輸入攝影時相對航高：);scanf(%f=lf,&H);printf(請輸入左像片的外方位元素初始值：n);printf(Xs1= );scanf(%lf,&Xs1);printf(Y

35、s1= );scanf(%lf,&Ys1);printf(Zs1= );scanf(%lf,&Zs1);printf(fai1= );scanf(%lf,&fai1);printf(oumiga1= );scanf(%lf,&oumiga1);printf(kapa1= );scanf(%lf,&kapa1);printf(請輸入右像片的外方位元素初始值：n);printf(Xs2= );scanf(%lf,&Xs2);printf(Ys2= );scanf(%lf,&Ys2);printf(Zs2= );scanf(%lf,&Zs2);printf(fai2= );scanf(%lf,&fa

36、i2);printf(oumiga2= );scanf(%lf,&oumiga2);printf(kapa2= );scanf(%lf,&kapa2);int diedainumber=0;/double Xunkonwen=0.0,Yunkonwen=0.0,Zunkonwen=0.0; /待定點的坐標(biāo)初始化double X=0.0,Y=0.0,Z=0.0,L161=0.0,A1612=0.0; /先求兩張相片的外方位元素double xcorrect121=0.0,AT1216=0.0,ATA1212=0.0,ATL121=0.0;double R133=0.0;double R233=0

37、.0;/-僅計算左右像片的外方位元素，沒有考慮位置點-do diedainumber+;/-組成左影像旋轉(zhuǎn)矩陣-R100=cos(fai1)*cos(kapa1)-sin(fai1)*sin(oumiga1)*sin(kapa1);R101=-cos(fai1)*sin(kapa1)-sin(fai1)*sin(oumiga1)*cos(kapa1);R102=-sin(fai1)*cos(oumiga1);R110=cos(oumiga1)*sin(kapa1);R111=cos(oumiga1)*cos(kapa1);R112=-sin(oumiga1);R120=sin(fai1)*co

38、s(kapa1)+cos(fai1)*sin(oumiga1)*sin(kapa1);R121=-sin(fai1)*sin(kapa1)+cos(fai1)*sin(oumiga1)*cos(kapa1);R122=cos(fai1)*cos(oumiga1);/-/計算系數(shù)陣和常數(shù)項for(int i=0,k=0,j=0;irow;i+,k+,j+)X=R100*(controlpointi*7+4-Xs1)+R110*(controlpointi*7+5-Ys1)+R120*(controlpointi*7+6-Zs1);Y=R101*(controlpointi*7+4-Xs1)+R111*(controlpointi*7+5-Ys1)+R121*(controlpointi*7+6-Zs1);Z=R102*(controlpointi*7+4-Xs1)+R112*(controlpointi*7+5-Ys1)+R122*(controlpointi*7+6-Zs1);Lj0=controlpointi*7+0+f*X/Z;Lj+10=cont