摄影测量程序汇总(后方交会前方交会单模型光束法平差)

程序运行环境为Visual Studio2010.运行前请先将坐标数据放在debug 下。

1.单像空间后方交会C语言程序：#include <stdio.h>#include <math.h>#include <iostream>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);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;k<n;k++){for(i=0;i<n;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;i<n;i++){if(i!=k){for(j=0;j<n;j++){if(j!=k)*(a+i*n+j)+=*(a+k*n+j)* *(a+i*n+k);}}}for(j=0;j<n;j++){if(j!=k)*(a+k*n+j)*=*(a+k*n+k);}}}void transpose(double *m1,double *m2,int m,int n) //矩阵转置{ int i,j;for(i=0;i<m;i++)for(j=0;j<n;j++)m2[j*m+i]=m1[i*n+j];return;}void multi(double *mat1,double *mat2,double * result,int a,int b,int c) { int i,j,k;for(i=0;i<a;i++){for(j=0;j<c;j++){result[i*c+j]=0;for(k=0;k<b;k++)result[i*c+j]+=mat1[i*b+k]*mat2[k*c+j];}}return;}double *readdata(){FILE *fp;int i,j;int number;char datacatolog[100];//scanf("%s",datacatolog);if ((fp=fopen("控制点坐标.txt","r"))==NULL){printf("读取数据出错!\n");return false;}fscanf(fp,"%d",&number);double *cordata=new double[number*5];for (i=0;i<number;i++){for (j=0;j<5;j++){fscanf(fp,"%lf",cordata+i*5+j);}}printf("控制点坐标数据读取成功！\n");return cordata;}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="结算数据.txt";fp=fopen(file1,"w");fprintf(fp,"---------原始坐标数据为--------：\n");for (int i=0;i<hang;i++){for (int j=0;j<5;j++){fprintf(fp,"%7.4lf ",data[i*5+j]);}fprintf(fp,"\n");}fprintf(fp,"--------------------------------\n");fprintf(fp,"---------误差方程系数阵为：--------：\n");for (int i=0;i<hang*2;i++){for (int j=0;j<6;j++){fprintf(fp,"%7.4lf ",xishuarray[i*5+j]);}fprintf(fp,"\n");}fprintf(fp,"--------------------------------\n");fprintf(fp,"---------法方程系数阵为：--------：\n");for (int i=0;i<6;i++){for (int j=0;j<6;j++){fprintf(fp,"%7.5lf ",faxishu[i*5+j]);}fprintf(fp,"\n");}fprintf(fp,"--------------------------------\n");fprintf(fp,"---------误差方程常数项为:--------：\n");for (int i=0;i<hang*2;i++){fprintf(fp,"%lf ",l[i]);fprintf(fp,"\n");}fprintf(fp,"--------------------------------\n");fprintf(fp,"---------迭代次数为:--------：\n");fprintf(fp,"%d\n",i);fprintf(fp,"--------------------------------\n");fprintf(fp,"-----------外方位元素为：---------\n");fprintf(fp," Xs= %lf, Ys=%lf, Zs=%lf\n",xs,ys,zs);fprintf(fp," fai= %lf, oumiga=%lf, kapa=%lf\n",fai,oumiga,kapa);fprintf(fp,"--------------------------------\n");fclose(fp);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 R[3][3]={0.0};double X=0.0,Y=0.0,Z=0.0,L[8][1]={0.0},A[8][6]={0.0};double correct[6][1]={0.0},AT[6][8]={0.0},ATA[6][6]={0.0},ATL[6][1]={0.0};int row; //row用于存放坐标行数double *controlpoint;controlpoint=readdata();row=sizeof(controlpoint);for (i=0;i<row;i++){for (j=0;j<5;j++){printf("%3.3lf ",controlpoint[i*5+j]);}printf("\n");}/*----------内方位元素----------*/printf("请输入像片的内方位元素(mm)：\n");printf("x0=");x0/=1000.0;scanf("%lf",&x0); //double类型数据要用%lfprintf("y0=");y0/=1000.0;scanf("%lf",&y0);printf("f=");scanf("%lf",&f);f=f/1000.0;/*------------------------------*///-------确定未知数初始值------for(int i=0;i<row;i++){Xs=Xs+controlpoint[i*5+2];Ys=Ys+controlpoint[i*5+3];Zs=Zs+controlpoint[i*5+4];}Xs/=row;Ys/=row;Zs=Zs/row+m*f;//-----------------------------do{diedainumber++;//---------组成旋转矩阵--------R[0][0]=cos(fai)*cos(kapa)-sin(fai)*sin(oumiga)*sin(kapa);R[0][1]=-cos(fai)*sin(kapa)-sin(fai)*sin(oumiga)*cos(kapa);R[0][2]=-sin(fai)*cos(oumiga);R[1][0]=cos(oumiga)*sin(kapa);R[1][1]=cos(oumiga)*cos(kapa);R[1][2]=-sin(oumiga);R[2][0]=sin(fai)*cos(kapa)+cos(fai)*sin(oumiga)*sin(kapa);R[2][1]=-sin(fai)*sin(kapa)+cos(fai)*sin(oumiga)*cos(kapa);R[2][2]=cos(fai)*cos(oumiga);//-----------------------------//计算系数阵和常数项for(int i=0,k=0,j=0;i<=3;i++,k++,j++){X=R[0][0]*(controlpoint[i*5+2]-Xs)+R[1][0]*(controlpoint[i*5+3]-Ys)+R[2][0]*(co ntrolpoint[i*5+4]-Zs);Y=R[0][1]*(controlpoint[i*5+2]-Xs)+R[1][1]*(controlpoint[i*5+3]-Ys)+R[2][1]*(con trolpoint[i*5+4]-Zs);Z=R[0][2]*(controlpoint[i*5+2]-Xs)+R[1][2]*(controlpoint[i*5+3]-Ys)+R[2][2]*(controlpoint[i*5+4]-Zs);L[j][0]=controlpoint[i*5+0]-(x0-f*X/Z);L[j+1][0]=controlpoint[i*5+1]-(y0-f*Y/Z);j++;A[k][0]=(R[0][0]*f+R[0][2]*(controlpoint[i*5+0]-x0))/Z;A[k][1]=(R[1][0]*f+R[1][2]*(controlpoint[i*5+0]-x0))/Z;A[k][2]=(R[2][0]*f+R[2][2]*(controlpoint[i*5+0]-x0))/Z;A[k][3]=(controlpoint[i*5+1]-y0)*sin(oumiga)-((controlpoint[i*5+0]-x0)*((control point[i*5+0]-x0)*cos(kapa)-(controlpoint[i*5+1]-y0)*sin(kapa))/f+f*cos(kapa))*cos(o umiga);A[k][4]=-f*sin(kapa)-(controlpoint[i*5+0]-x0)*((controlpoint[i*5+0]-x0)*sin(kapa) +(controlpoint[i*5+1]-y0)*cos(kapa))/f;A[k][5]=controlpoint[i*5+1]-y0;A[k+1][0]=(R[0][1]*f+R[0][2]*(controlpoint[i*5+1]-y0))/Z;A[k+1][1]=(R[1][1]*f+R[1][2]*(controlpoint[i*5+1]-y0))/Z;A[k+1][2]=(R[2][1]*f+R[2][2]*(controlpoint[i*5+1]-y0))/Z;A[k+1][3]=-(controlpoint[i*5+0]-x0)*sin(oumiga)-((controlpoint[i*5+1]-y0)*((cont rolpoint[i*5+0]-x0)*cos(kapa)-(controlpoint[i*5+1]-y0)*sin(kapa))/f-f*sin(kapa))*cos( oumiga);A[k+1][4]=-f*cos(kapa)-(controlpoint[i*5+1]-y0)*((controlpoint[i*5+0]-x0)*sin(ka pa)+(controlpoint[i*5+1]-y0)*cos(kapa))/f;A[k+1][5]=-(controlpoint[i*5+0]-x0);k++;}transpose(A[0],AT[0],8,6);multi(AT[0],A[0],ATA[0],6,8,6);inverse(ATA[0],6);multi(AT[0],L[0],ATL[0],6,8,1);multi(ATA[0],ATL[0],correct[0],6,6,1);Xs=Xs+correct[0][0];Ys=Ys+correct[1][0];Zs=Zs+correct[2][0];fai=fai+correct[3][0];oumiga=oumiga+correct[4][0];kapa=kapa+correct[5][0];}while(correct[3][0]>=6.0/206265.0||correct[4][0]>=6.0/206265.0||correct[5][0] >=6.0/206265.0);printf("迭代次数为：%d\n",diedainumber);printf("---------误差方程系数为：--------\n");for (i=0;i<8;i++){for (j=0;j<6;j++){printf("%4.4lf ",A[i][j]);}printf("\n");}printf("--------------------------------\n");printf("求解得到的外方位元素为：\n");printf(" Xs= %lf\n",Xs);printf(" Ys= %lf\n",Ys);printf(" Zs= %lf\n",Zs);printf(" fai= %lf\n",fai);printf(" oumiga= %lf\n",oumiga);printf(" kapa= %lf\n",kapa);savedata(row,controlpoint,A[0],ATA[0],L[0],diedainumber,Xs,Ys,Zs,fai,oumiga,kap a);printf("-----------------解算结束！--------------\n");system("pause");}解算结果：2.后方交会-前方交会求解地面点坐标已知左右像片外方位元素，给出像点坐标：左像点坐标：右像点坐标：x（/m）y（/m）x（/m）y（/m）0.0053 0.0069 0.00482 0.00270.00556 0.00229 0.00521 -0.001910.00815 0.00499 0.00773 0.00089-0.00247 0.00156 -0.00279 -0.00292 C语言代码：#include <stdio.h>#include <iostream>#include <math.h>double *readdata();void savedata(int hang,double *data);double *readdata(){FILE *fp;int i,j,k;int number;char datacatolog[100];char leftdata[300];//scanf("%s",datacatolog);if ((fp=fopen("像点坐标数据.txt","r"))==NULL){printf("读取数据出错!\n");system("pause");exit(0);}fscanf(fp,"%d",&number);double *c=new double[number*4];for (k=0;k<2;k++){fread(&leftdata,14,1,fp);for (i=0;i<number;i++){for (j=0;j<2;j++){fscanf(fp,"%lf",c+k*2+i*4+j);}}}fclose(fp);return c;}void savedata(int hang,double *data){FILE *fp;char *file1="地面点坐标数据.txt";fp=fopen(file1,"w");fprintf(fp,"---------像点对应地面点坐标为--------：\n");fprintf(fp,"\n");for (int i=0;i<hang;i++){fprintf(fp,"第%d点：",i+1);for (int j=0;j<3;j++){fprintf(fp,"%7.4lf ",data[i*3+j]);}fprintf(fp,"\n\n");}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=1486.212;doublefai2=0.0063,oumiga2=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);//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=0,Z1=0,X2=0,Y2=0,Z2=0;double R1[3][3]={0.0};double R2[3][3]={0.0};double GEOdata[4][3]={0.0};for (i=0;i<row;i++){//---------组成左影像旋转矩阵--------R1[0][0]=cos(fai1)*cos(kapa1)-sin(fai1)*sin(oumiga1)*sin(kapa1);R1[0][1]=-cos(fai1)*sin(kapa1)-sin(fai1)*sin(oumiga1)*cos(kapa1);R1[0][2]=-sin(fai1)*cos(oumiga1);R1[1][0]=cos(oumiga1)*sin(kapa1);R1[1][1]=cos(oumiga1)*cos(kapa1);R1[1][2]=-sin(oumiga1);R1[2][0]=sin(fai1)*cos(kapa1)+cos(fai1)*sin(oumiga1)*sin(kapa1);R1[2][1]=-sin(fai1)*sin(kapa1)+cos(fai1)*sin(oumiga1)*cos(kapa1);R1[2][2]=cos(fai1)*cos(oumiga1);//-----------------------------------//---------组成右影像旋转矩阵--------R2[0][0]=cos(fai2)*cos(kapa2)-sin(fai2)*sin(oumiga2)*sin(kapa2);R2[0][1]=-cos(fai2)*sin(kapa2)-sin(fai2)*sin(oumiga2)*cos(kapa2);R2[0][2]=-sin(fai2)*cos(oumiga2);R2[1][0]=cos(oumiga2)*sin(kapa2);R2[1][1]=cos(oumiga2)*cos(kapa2);R2[1][2]=-sin(oumiga2);R2[2][0]=sin(fai2)*cos(kapa2)+cos(fai2)*sin(oumiga2)*sin(kapa2);R2[2][1]=-sin(fai2)*sin(kapa2)+cos(fai2)*sin(oumiga2)*cos(kapa2);R2[2][2]=cos(fai2)*cos(oumiga2);//-------------像空辅系坐标-------------X1=R1[0][0]*imagepoint[i*4+0]+R1[0][1]*imagepoint[i*4+1]-R1[0][2]*f;Y1=R1[1][0]*imagepoint[i*4+0]+R1[1][1]*imagepoint[i*4+1]-R1[1][2]*f;Z1=R1[2][0]*imagepoint[i*4+0]+R1[2][1]*imagepoint[i*4+1]-R1[2][2]*f;X2=R2[0][0]*imagepoint[i*4+2]+R2[0][1]*imagepoint[i*4+3]-R2[0][2]*f;Y2=R2[1][0]*imagepoint[i*4+2]+R2[1][1]*imagepoint[i*4+3]-R2[1][2]*f;Z2=R2[2][0]*imagepoint[i*4+2]+R2[2][1]*imagepoint[i*4+3]-R2[2][2]*f;//--------------------------------------//------------点投影系数-------------N1=(Bx*Z2-Bz*X2)/(X1*Z2-Z1*X2);N2=(Bx*Z1-Bz*X1)/(X1*Z2-Z1*X2);//-----------------------------------//------------计算地面点坐标------------GEOdata[i][0]=Xs1+N1*X1;GEOdata[i][1]=Ys1+By+N2*Y2;GEOdata[i][2]=Zs1+N1*Z1;//--------------------------------------}//--------------------------------------------for (i=0;i<4;i++){printf("第%d个地面点坐标：",i+1);for (j=0;j<3;j++){printf("%lf ",GEOdata[i][j]);}printf("\n\n");}savedata(row,GEOdata[0]);system("pause");}测试结果：3.单模型光束法严密平差缺少已知数据进行验证，因此如果有已知数据请代入已知数据进行验证。