摄影测量后方交会程序

摄影测量后方交会程序（c/c++）

输入数据截图：

结果截图：

程序源代码（其中的矩阵求逆在前面已经有了，链接）：

#include

#include

#include

const double PRECISION=1e-5;

typedef double DOUBLE[5];

int InputData(int &Num, DOUBLE *&Data,double &m,double &f);

int Resection(const int &Num,const DOUBLE *&Data,const double &m,const double &f); int InverseMatrix(double *matrix,const int &row);

int main(int argc, char* argv[])

{

DOUBLE *Data=NULL;

int Num;

double f(0),m(0);

if(InputData(Num,Data,m,f))

{

if (Data!=NULL)

{

delete []Data;

}

return 1;

}

if(Resection(Num,Data,m,f))

{

if (Data!=NULL)

{

delete []Data;

}

return 1;

}

if (Data!=NULL)

{

delete []Data;

}

printf("解算完毕...\n");

do{

printf("计算结果保存于\"结果.txt\"文件中\n"

"请选择操作（输入P打开结果数据，R打开原始数据，其它退出程序）："); fflush(stdin); //刷新输入流

char order=getchar();

if ('P'==order || 'p'==order)

{

system("结果.txt");

}

else if ('R'==order || 'r'==order)

{

system("data.txt");

}

else

break;

system("cls");

}while(1);

system("PAUSE");

return 0;

}

/**********************************************

*函数名：InputData

*函数介绍：从文件（data.txt）中读取数据,

*文件格式如下：

*点数 m(未知写作0)

* 内方位元素(f x0 y0)

*编号 x y X Y Z

*下面是一个实例：

4 0

153.24 0 0

1 -86.15 -68.99 36589.41 25273.3

2 2195.17

2 -53.40 82.21 37631.08 31324.51 728.69

3 -14.78 -76.63 39100.97 24934.98 2386.50

4 10.46 64.43 40426.54 30319.81 757.31

*参数：(in/out)Num(点数)，

*(in/out)Data(存放数据),m,f,x0,y0

*返回值：int ，0成功，1文件打开失败，2控制点个

*数不足，3文件格式错误

*作者：vcrs

*完成时间：09-10-4

**********************************************/

int InputData(int &Num, DOUBLE *&Data,double &m,double &f) {

double x0,y0;

FILE *fp_input;

if (!(fp_input=fopen("data.txt","r")))

{

return 1;

}

fscanf(fp_input,"%d%lf",&Num,&m);

if (Num<4)

{

return 2;

}

fscanf(fp_input,"%lf%lf%lf",&f,&x0,&y0);

f/=1000;

if (m<0 || f<0)

{

return 3;

}

Data=new DOUBLE[Num];

double *temp= new double[Num-1];

double scale=0;

int i;

for (i=0;i

{

//读取数据，忽略编号

if(fscanf(fp_input,"%*d%lf%lf%lf%lf%lf",

&Data[i][0],&Data[i][1],&Data[i][2],

&Data[i][3],&Data[i][4])!=5)

{

return 3;

}

//单位换算成m

Data[i][0]/=1000.0;

Data[i][1]/=1000.0;

}

//如果m未知则归算其值

if (0==m)

{

for (i=0;i

{

temp[i]=(Data[i][2]-Data[i+1][2])/(Data[i][0]-Data[i+1][0])+ (Data[i][3]-Data[i+1][3])/(Data[i][1]-Data[i+1][1]);

scale+=temp[i]/2.0;

}

m=scale/(Num-1);

}

fclose(fp_input);

delete []temp;

return 0;

}

/**********************************************

*函数名：MatrixMul

*函数介绍：求两个矩阵的积,

*参数：Jz1(第一个矩阵),row(第一个矩阵行数),

*Jz2(第二个矩阵),row(第二个矩阵列数),com(第一个

*矩阵列数),(out)JgJz(存放结果矩阵)

*返回值：void

*作者：vcrs

*完成时间：09-10-4

**********************************************/

void MatrixMul(double *Jz1,const int &row,double *Jz2,

const int &line,const int &com,double *JgJz)

{

for (int i=0;i

{

for (int j=0;j

{

double temp=0;

for (int k=0;k

{

temp+=*(Jz1+i*com+k)*(*(Jz2+k*line+j));

}

*(JgJz+i*line+j)=temp;

}

}

}

/**********************************************

*函数名：OutPut

*函数介绍：向结果.txt文件输出数据

*参数：Q协因数阵，m精度，m0单位权中误差，6个外

*方位元素，旋转矩阵

*返回值：int，0成功，1失败

*作者：vcrs

*完成时间：09-10-4

**********************************************/

int OutPut(const double *&Q,const double *&m,const double &m0, const double &Xs,const double &Ys,const double &Zs,

const double &Phi,const double &Omega,

const double &Kappa,const double *R)

{

FILE *fp_out;

if (!(fp_out=fopen("结果.txt","w")))

{

return 1;

}

FILE *fp_input;

if (!(fp_input=fopen("data.txt","r")))

{

return 1;

}

fprintf(fp_out,"**************************************"

"**************************************"

"**************************************"

"*********************************\n");

fprintf(fp_out,"\n空间后方交会程序(C\\C++)\n遥感信息工程学院\n班级：" "00000\n学号：0000000\n姓名：vcrs\n\n");

fprintf(fp_out,"**************************************"

"**************************************"

"**************************************"

"*********************************\n");

fprintf(fp_out,"已知数据：\n\n已知点数：");

int num;double temp,x,y;

fscanf(fp_input,"%d%lf",&num,&temp);

fprintf(fp_out,"%d\n",num);

fprintf(fp_out,"摄影比例尺（0表示其值位置）：");

fprintf(fp_out,"%10.0lf\n",temp);

fprintf(fp_out,"内方位元素(f x0 y0)：");

fscanf(fp_input,"%lf%lf%lf",&temp,&x,&y);

fprintf(fp_out,"%10lf\t%10lf\t%10lf\n",temp,x,y);

for (int i=0;i

{

double temp[5];

fscanf(fp_input,"%*d%lf%lf%lf%lf%lf",

&temp[0],&temp[1],&temp[2],&temp[3],&temp[4]);

fprintf(fp_out,"%3d\t%10lf\t%10lf\t%10lf\t%10lf\t%10lf\n", i+1,temp[0],temp[1],temp[2],temp[3],temp[4]);

}

fclose(fp_input);

fprintf(fp_out,"**************************************" "**************************************"

"**************************************"

"*********************************\n");

fprintf(fp_out,"计算结果如下：\n\n外方位元素：\n");

fprintf(fp_out,"\tXs=%10lf\n",Xs);

fprintf(fp_out,"\tYs=%10lf\n",Ys);

fprintf(fp_out,"\tZs=%10lf\n",Zs);

fprintf(fp_out,"\tPhi=%10lf\n",Phi);

fprintf(fp_out,"\tOmega=%10lf\n",Omega);

fprintf(fp_out,"\tKappa=%10lf\n\n",Kappa);

fprintf(fp_out,"旋转矩阵：\n");

for (i=0;i<3;i++)

{

fprintf(fp_out,"\t");

for (int j=0;j<3;j++)

{

fprintf(fp_out,"%10lf\t",*(R+i*3+j));

}

fprintf(fp_out,"\n");

}

fprintf(fp_out,"\n单位权中误差：%10lf\n\n",m0);

fprintf(fp_out,"协因数阵：\n");

for (i=0;i<6;i++)

{

fprintf(fp_out,"\t");

for (int j=0;j<6;j++)

{

fprintf(fp_out,"%20lf\t",*(Q+i*6+j));

}

fprintf(fp_out,"\n");

}

fprintf(fp_out,"\n外方位元素精度：");

for (i=0;i<6;i++)

{

fprintf(fp_out,"%10lf\t",m[i]);

}

fprintf(fp_out,"\n");

fprintf(fp_out,"**************************************"

"**************************************"

"**************************************"

"*********************************\n");

fclose(fp_out);

return 0;

}

/**********************************************

*函数名：Resection

*函数介绍：计算

*参数：Num(点数)，Data(数据)，m,,f(焦距),x0,y0

*返回值：int，0成功，其它失败

*作者：vcrs

*完成时间：09-10-4

**********************************************/

int Resection(const int &Num,const DOUBLE *&Data,const double &m, const double &f)

{

double Xs=0,Ys=0,Zs=0;

int i,j;

//设置初始值

for (i=0;i

{

Xs+=Data[i][2];

Ys+=Data[i][3];

}

Xs/=Num;

Ys/=Num;

Zs=m*f;

double Phi(0),Omega(0),Kappa(0);

double R[3][3]={0.0};

double *L=new double[2*Num];

typedef double Double6[6];

Double6 *A=new Double6[2*Num];

double *AT=new double[2*Num*6];

double *ATA=new double[6*6];

double *ATL=new double[6];

double *Xg=new double[6];

//迭代计算

do

{

//旋转矩阵

R[0][0]=cos(Phi)*cos(Kappa)-sin(Phi)*sin(Omega)*sin(Kappa); R[0][1]=-cos(Phi)*sin(Kappa)-sin(Phi)*sin(Omega)*cos(Kappa); R[0][2]=-sin(Phi)*cos(Omega);

R[1][0]=cos(Omega)*sin(Kappa);

R[1][1]=cos(Omega)*cos(Kappa);

R[1][2]=-sin(Omega);

R[2][0]=sin(Phi)*cos(Kappa)+cos(Phi)*sin(Omega)*sin(Kappa);

R[2][1]=-sin(Phi)*sin(Kappa)+cos(Phi)*sin(Omega)*cos(Kappa); R[2][2]=cos(Phi)*cos(Omega);

for (i=0;i

{

double X=R[0][0]*(Data[i][2]-Xs)+R[1][0]*(Data[i][3]-Ys)+

R[2][0]*(Data[i][4]-Zs);

double Y=R[0][1]*(Data[i][2]-Xs)+R[1][1]*(Data[i][3]-Ys)+

R[2][1]*(Data[i][4]-Zs);

double Z=R[0][2]*(Data[i][2]-Xs)+R[1][2]*(Data[i][3]-Ys)+

R[2][2]*(Data[i][4]-Zs);

double xxx,yyy;

xxx=-f*X/Z;

yyy=-f*Y/Z;

//常数项

L[2*i]=Data[i][0]-(-f*X/Z);

L[2*i+1]=Data[i][1]-(-f*Y/Z);

A[2*i][0]=(R[0][0]*f+R[0][2]*(xxx))/Z;

A[2*i][1]=(R[1][0]*f+R[1][2]*(xxx))/Z;

A[2*i][2]=(R[2][0]*f+R[2][2]*(xxx))/Z;

A[2*i][3]=(yyy)*sin(Omega)-(((xxx)/f)*

((xxx)*cos(Kappa)-(yyy)*sin(Kappa))+

f*cos(Kappa))*cos(Omega);

A[2*i][4]=-f*sin(Kappa)-((xxx)/f)*((xxx)*

sin(Kappa)+(yyy)*cos(Kappa));

A[2*i][5]=(yyy);

A[2*i+1][0]=(R[0][1]*f+R[0][2]*(yyy))/Z;

A[2*i+1][1]=(R[1][1]*f+R[1][2]*(yyy))/Z;

A[2*i+1][2]=(R[2][1]*f+R[2][2]*(yyy))/Z;

A[2*i+1][3]=-(xxx)*sin(Omega)-(((yyy)/f)*

((xxx)*cos(Kappa)-(yyy)*sin(Kappa))-

f*sin(Kappa))*cos(Omega);

A[2*i+1][4]=-f*cos(Kappa)-((yyy)/f)*((xxx)*

sin(Kappa)+(yyy)*cos(Kappa));

A[2*i+1][5]=-(xxx);

}

//求矩阵A的转置矩阵AT

for (i=0;i<2*Num;i++)

{

for (j=0;j<6;j++)

{

*(AT+j*2*Num+i)=A[i][j];

}

}

//求ATA

MatrixMul(AT,6,&A[0][0],6,2*Num,ATA);

if(InverseMatrix(ATA,6))

return 1;

MatrixMul(AT,6,L,1,2*Num,ATL);

MatrixMul(ATA,6,ATL,1,6,Xg);

Xs+=Xg[0];

Ys+=Xg[1];

Zs+=Xg[2];

Phi+=Xg[3];

Omega+=Xg[4];

Kappa+=Xg[5];

} while(fabs(Xg[0])>=PRECISION ||fabs(Xg[1])>=PRECISION || fabs(Xg[2])>=PRECISION ||fabs(Xg[3])>=PRECISION ||

fabs(Xg[4])>=PRECISION || (Xg[5])>=PRECISION);

//注：协因数阵，旋转矩阵等计算本应该使用最后外方位元素值，//由于变换很小忽略

double *Q=ATA;

double *V=new double[2*Num];

MatrixMul(&A[0][0],2*Num,Xg,1,6,V);

double VTV=0;

for(i=0;i<2*Num;i++)

{

V[i]-=L[i];

VTV+=V[i]*V[i];

}

double m0=sqrt(VTV/(2*Num-6));

double *mm=new double[6];

for (i=0;i<6;i++)

{

mm[i]=sqrt(*(Q+i*6+i))*m0;

}

OutPut(Q,mm,m0,Xs,Ys,Zs,Phi,Omega,Kappa,&R[0][0]);

delete []L;

delete []A;

delete []AT;

delete []ATA;

delete []ATL;

delete []Xg;

delete []mm;

delete []V;

return 0;

}

void swap(double &a,double &b)

{

double temp=a;

a=b;

b=temp;

}

/********************************************** *函数名：InverseMatrix

*函数介绍：求矩阵的逆（高斯-约当法）

*输入参数：(in/out)matrix（矩阵首地址），

*（in）row（矩阵阶数）

*输出参数：matrix（原矩阵的逆矩阵）

*返回值：int ，0成功，1失败

*调用函数：swap(double&,double&)

*作者：vcrs

*完成时间：09-10-4

**********************************************/ int InverseMatrix(double *matrix,const int &row) {

double *m=new double[row*row];

double *ptemp,*pt=m;

int i,j;

ptemp=matrix;

for (i=0;i

{

for (j=0;j

{

*pt=*ptemp;

ptemp++;

pt++;

}

}

int k;

int *is=new int[row],*js=new int[row];

for (k=0;k

{

double max=0;

//全选主元

//寻找最大元素

for (i=k;i

{

for (j=k;j

{

if (fabs(*(m+i*row+j))>max)

{

max=*(m+i*row+j);

is[k]=i;

js[k]=j;

}

}

}

if (0 == max)

{

return 1;

}

//行交换

if (is[k]!=k)

{

for (i=0;i

{

swap(*(m+k*row+i),*(m+is[k]*row+i)); }

}

//列交换

if (js[k]!=k)

{

for (i=0;i

{

swap(*(m+i*row+k),*(m+i*row+js[k])); }

}

*(m+k*row+k)=1/(*(m+k*row+k));

for (j=0;j

{

if (j!=k)

{

*(m+k*row+j)*=*((m+k*row+k));

}

}

for (i=0;i

{

if (i!=k)

{

for (j=0;j

{

if(j!=k)

{

*(m+i*row+j)-=*(m+i*row+k)**(m+k*row+j); }

}

}

}

for (i=0;i

{

if(i!=k)

{

*(m+i*row+k)*=-(*(m+k*row+k));

}

}

}

int r;

//恢复行列

for (r=row-1;r>=0;r--)

{

if (js[r]!=r)

{

for (j=0;j

{

swap(*(m+r*row+j),*(m+js[r]*row+j)); }

}

if (is[r]!=r)

{

for (i=0;i

{

swap(*(m+i*row+r),*(m+i*row+is[r])); }

}

}

ptemp=matrix;

pt=m;

for (i=0;i

{

for (j=0;j

{

*ptemp=*pt;

ptemp++;

pt++;

}

}

delete []is;

delete []js;

delete []m;

return 0;

}