攝影測(cè)量程序匯總(后方交會(huì)+前方交會(huì)+單模型光束法平差)

1、程序運(yùn)行環(huán)境為Visual Studio2010.運(yùn)行前請(qǐng)先將坐標(biāo)數(shù)據(jù)放在debug下。1. 單像空間后方交會(huì)原始數(shù)據(jù)：內(nèi)方位元素x0(/mm)y0(/mm)主距 f(/mm)比例尺分母00153.2450000像點(diǎn)坐標(biāo) (/mm)物點(diǎn)坐標(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 語(yǔ)言程序：#include #include #include double *

2、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); void transpose(double *m1,double *m2,int m,int n);void inverse(double *a,int n);void multi(double *mat1,double * mat2,double * result

3、,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+)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 tr

4、anspose(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 *read

5、data()FILE *fp;int i,j;int number;char datacatolog100;/scanf(%s,datacatolog);if (fp=fopen( 控制點(diǎn)坐標(biāo) .txt,r)=NULL)printf( 讀取數(shù)據(jù)出錯(cuò) !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( 控制點(diǎn)坐標(biāo)數(shù)據(jù)讀取成功！n);return cordata

6、;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)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

7、);fprintf(fp,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(f

8、p,- 誤差方程常數(shù)項(xiàng)為 :-：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)

9、;return;void 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;c

10、ontrolpoint=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( 請(qǐng)輸入像片的內(nèi)方位元素(mm)：n);printf(x0=);x0/=1000.0;scanf(%lf,&x0);/double 類(lèi)型數(shù)據(jù)要用 %lfprintf(y0=);y0/=1000.0;scanf(%lf,&y0);printf(f=);scanf(%lf,&f);f=f/1000.0;/*-*/-

11、確定未知數(shù)初始值 -for(int 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;/-dodiedainumber+;/- 組成旋轉(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=

12、cos(oumiga)*sin(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);/-/ 計(jì)算系數(shù)陣和常數(shù)項(xiàng)for(int i=0,k=0,j=0;i=6.0/ 206265.0|correct40=6.0/206265.0|correct50 =6.0/206265.0);prin

13、tf( 迭代次數(shù)為： %dn,diedainumber);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,con

14、trolpoint,A0,ATA0,L0,diedainumber,Xs,Ys,Zs,fai,oumiga,kapa);printf(- 解算結(jié)束！ -n);system(pause);解算結(jié)果：2. 后方交會(huì) -前方交會(huì)求解地面點(diǎn)坐標(biāo)已知左右像片外方位元素，給出像點(diǎn)坐標(biāo)：左像點(diǎn)坐標(biāo)：右像點(diǎn)坐標(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 語(yǔ)言代碼：#include

15、 #include #include double *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( 像點(diǎn)坐標(biāo)數(shù)據(jù) .txt,r)=NULL)printf( 讀取數(shù)據(jù)出錯(cuò)!n);system(pause);exit(0);fscanf(fp,%d,&number);double *c=new doublen

16、umber*4;for (k=0;k2;k+)fread(&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= 地面點(diǎn)坐標(biāo)數(shù)據(jù).txt;fp=fopen(file1,w);fprintf(fp,- 像點(diǎn)對(duì)應(yīng)地面點(diǎn)坐標(biāo)為fprintf(fp,n);for (int i=0;ihang;i+)-： n);fprintf(fp, 第 %d

17、點(diǎn)：for (int j=0;j3;j+),i+1);fprintf(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;doublefai1=-0.0061,oumiga1=0.0327,kapa1=0.1711,Ys1=397367.171,Xs1=3445820.098,Zs1=14

18、86.212; doublefai2=0.0063,oumiga2=0.0178,kapa2=0.1489,Ys2=397367.234,Xs2=3445959.266,Zs2=1490.096;/ printf( 請(qǐng)輸入左像片的外方位元素： 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);/p

19、rintf(kapa1= );/scanf(%lf,&kapa1);/printf( 請(qǐng)輸入右像片的外方位元素：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

20、-Zs1;double N1=0,N2=0;double X1=0,Y1=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

21、(oumiga1)*sin(kapa1);R111=cos(oumiga1)*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=-c

22、os(fai2)*sin(kapa2)-sin(fai2)*sin(oumiga2)*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(

23、oumiga2);/- 像空輔系坐標(biāo)-X1=R100*imagepointi*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*imagepoi

24、nti*4+2+R221*imagepointi*4+3-R222*f; /-/- 點(diǎn)投影系數(shù) -N1=(Bx*Z2-Bz*X2)/(X1*Z2-Z1*X2);N2=(Bx*Z1-Bz*X1)/(X1*Z2-Z1*X2);/-/- 計(jì)算地面點(diǎn)坐標(biāo)-GEOdatai0=Xs1+N1*X1;GEOdatai1=Ys1+By+N2*Y2;GEOdatai2=Zs1+N1*Z1;/-/-for (i=0;i4;i+)printf( 第 %d 個(gè)地面點(diǎn)坐標(biāo)：,i+1);for (j=0;j3;j+)printf(%lf,GEOdataij);printf(nn);savedata(row,GEOdata

25、0);system(pause);測(cè)試結(jié)果：3. 單模型光束法嚴(yán)密平差缺少已知數(shù)據(jù)進(jìn)行驗(yàn)證，因此如果有已知數(shù)據(jù)請(qǐng)代入已知數(shù)據(jù)進(jìn)行驗(yàn)證。C 語(yǔ)言代碼：#include #include double *readdata();void savedata(int hang,double *data,double*xishuarray,double*faxishu,double*l,inti,doublexs,double ys,double zs,double fai,double oumiga,double kapa,double xs2,double ys2,double zs2,double

26、fai2,double oumiga2,double kapa2);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 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)/(*

27、(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(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,d

28、ouble *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()FILE *fp;int i,j,k;int number;char datacatolog100;char leftdata300;/scanf(%s,datacatolog);if (fp=fopen( 控制點(diǎn)坐標(biāo) .txt,r)=NULL)printf

29、( 讀取數(shù)據(jù)出錯(cuò)!n);return false;fscanf(fp,%d,&number);double *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

30、,double*faxishu,double*l,inti,doublexs,double ys,doublezs,double fai,doubleoumiga,doublekapa,doublexs2,doubleys2,doublezs2,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,d

31、atai*7+j);fprintf(fp,n);fprintf(fp,-n);fprintf(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,-

32、n);fprintf(fp,- 誤差方程常數(shù)項(xiàng)為for (int i=0;ihang*4;i+):- ： n);fprintf(fp,%lffprintf(fp,n);,li);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);fp

33、rintf(fp,-n);fprintf(fp,-右像的外方位元素為： -n);fprintf(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;d

34、ouble x0left=0,y0left=0;double x0right=0,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( 請(qǐng)輸入攝影機(jī)焦距： scanf(%f=lf,&f); f=f/1000;);printf(請(qǐng)輸入攝影時(shí)相對(duì)航高：);scanf(%f=lf,&H);printf( 請(qǐng)輸入左像片的外方位元素

35、初始值：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);n);printf(請(qǐng)輸入右像片的外方位元素初始值：n);printf(Xs2= );scanf(%lf,&Xs2);printf(Ys2= );scanf(%lf,&Ys2);printf(Zs2= )

36、;scanf(%lf,&Zs2);printf(fai2= );scanf(%lf,&fai2);printf(oumiga2= );scanf(%lf,&oumiga2);printf(kapa2= );scanf(%lf,&kapa2);int diedainumber=0;/doubleXunkonwen=0.0,Yunkonwen=0.0,Zunkonwen=0.0;/ 待定點(diǎn)的坐標(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.0;/- 僅計(jì)算左右像片的外方位元素，沒(méi)有考慮位置點(diǎn)-dodiedainumber+;/- 組成左影像旋轉(zhuǎn)矩陣-R