空间后方交会C程序代码

摄影测量前方交会程序(C/C++)

输入数据截图：

结果截图：

程序源代码(其中的矩阵求逆在前面己经有了，链接)：

Sinclude<stdio.h>

^include<stdlib.h>

^include<math.h>

constdoublePRECISION=le-5;

typedefdoubleDOUBLE[5];

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

intResection(constint&Num,constDOUBLE*&Data,constdouble&m,constdouble&f);

intInverseMatrix(double*matrix,constint&row);

intmain(intargc,char*argv[])

(

DOUBLE*Data=NULL;

intNum;

doublef(0),m(0);

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

(

if(Data!=NULL)

(

delete[]Data;

)

return1;

)

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

(

if(Data!=NULL)

(

delete[jData;

}

return1;

)

if(Data!=NULL)

{

delete[]Data;

)

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

do{

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

〃请选择操作(输入P翻开结果数据，R翻开原始数据，其它退出程序)：〃);

fflush(stdin);〃刷新输入流

charorder=getchar();

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

(

system(〃结果.txt");

)

elseif('R'==order||*r*==order)

(

system(z，data.txt");

)

else

break;

system(〃cls〃)；

}while(l);

system("PAUSE");

return0;

)

/5^C^|C

*函数名：InputData

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

*文件格式如下：

*点数m（未知写作0）

*内方位元素（fxOyO）

*编号xyXYZ

*下面是一个实例：

40

153.2400

1-86.15-68.9936589.4125273.322195.17

2-53.4082.2137631.0831324.51728.69

3-14.78-76.6339100.9724934.982386.50

410.4664.4340426.5430319.81757.31

*参数：（in/out）Num（点数），

*（in/out）Data（存放数据），m,f,xO,yO

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

*数缺乏，3文件格式错误

*vers

*完成时间:09-10-4

.Z、］,、］,—I，、］■^r*、［,、1，/

intlnputData（int&Num,DOUBLE*&Data,double&m,double&f）

doublexO,yO;

FILE*fp_input;

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

(

return1;

}

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

if(Num<4)

{

return2;

}

fscanf(fpinput,&f,&x0,&y0);

f/=1000;

if(m<0||f<0)

(

return3;

}

Data=newDOUBLE[Num];

double*temp=newdouble[Num-11;

doublescale=0;

inti;

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

(

〃读取数据，忽略编号

if(fscanf(fp_input,

&Data[i][oj,&Data[i][l],&Data[i][2],

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

(

return3;

}

〃单位换算成m

Data[i][0]/=1000.0;

Data[i][l]/=1000.0;

)

〃如果m未知那么归算其值

if(0==m)

(

for(i=0;i<Num-l;i++)

(

temp[i]=(Data[i][2]-Data[i+l][2])/(Data[i][0]-Data[i+l][0])+

(Dala[i][3]-Data[i+l][3])/(Da.la[i][l]_Dalci[i+l][1]);

scale+=temp[i]/2.D;

}

m=scale/(Num-1);

}

fclose(fpinput);

delete[]temp;

return0;

)

；W*q.%lx、＞

，*T*^7**（*^J**^»^7*

*函数名：MatrixMul

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

*参数：Jzl（第一个矩阵），row（第一个矩阵行数），

*Jz2（第二个矩阵），row（第二个矩阵列数），com（第一个

*矩阵列数），（out）jgjz（存放结果矩阵）

*返回值:void

*vers

*完成时间:09-10-4

>jcx<>|<>}c>：<x<J{C>]c>卜x<x<>|<)}c>jcx<x《>k>k>：<x<^{<>卜>jcx<x<>jc>jc>：<x<^{c)卜>卜x<x<>|c>}c>：<x<x<>k>jcx<x<)卜/

voidMatrixMul(double*Jzl,constint&row,double*Jz2,

constint&line,constint&com,double*JgJz)

(

for(inti=0;i<row;i++)

(

for(intj=0;j<line;j++)

{

doubletemp=0;

for(intk=0;k<com;k++)

(

temp+=*(Jzl+i*com+k)*(*(Jz2+k*1ine+j));

}

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

)

}

)

/«/>

*T**T**7**(**T^*T**T*^7^*1**T**T**T**T**T**T**T**T**T**T**T**T**7**T^*T**T**1**T**7*

*函数名：OutPut

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

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

*方位元素，旋转矩阵

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

*vers

*完成时间:09-10-4

«y»ri*rT**J*»T**T*»Y»riw*T*rj*ri**J**y»ri*ri*rjw*1*»T*ri*rj*»Y**T*rj**1**J**y»ri**T»,,

intOutPut(constdouble*&Q,constdouble*&m,constdouble&m0,

constdouble&Xs,constdouble&Ys,constdouble&Zs,

constdouble&Phi?constdouble&0mega,

constdouble&Kappa,constdouble*R)

(

FILE*fp_out;

if(!(fp_out=fopen(〃结果.txt〃，〃w〃)))

(

return1;

}

FILE*fp_input;

if(!(fp_input=fopen(z/data.txl",〃r〃)))

(一

return1;

}

fprintf(fpout,〃**************************************〃

〃"

*T**r**r**i**r*^T»

X<J{C>]c>卜x<x<^jc>jc>：«x<x<>|c>|c>：<x<^{<}jc>jcx<X<>jc>：<x<^{c)卜>卜x<x《>jc>}c>：<x<x<>卜>jcx<x<

〃^|c^|c〃)•

fprintf(fp_out,"\n空间前方交会程序(C\\C++)\n遥感信息工程学院\n班级:

〃00000\n学号：0000000\n姓名:vcrs\n\n〃)；

fprintf(fpout,”**************************************"

*T**T»*T**T*^r**?*'

*^C^Jc5^C*^C5^C»^C*^Cr^CJ^C5|C5^C*^C*^C5^C5jw*^C

*T**7**T**T**T**T**1**T**T**T*\\I1、•

fprin贯(fp_uut,〃数据：\u\u点数：〃)；

intnum;doubletemp,x,y;

fscanf(fpinput,/z%d%lf/z,&num,&temp);

fprintf(fp_out,，，%d\n，/,num);

fprintf(fp_out,〃摄影比例尺(。表示其值位置)：〃)；

fprintf(fp_out,z，%10.01f\n”,temp);

fprintf(fp_out,〃内方位元素(fxOyO)：〃)；

fscanf(fpinput,，，%lf%lf%lf/，,&temp,&x,&y);

fprintf(fp_out,/z%101f\t%101f\t%101f\nz，,temp,x,y);

for(inti=0;i<num;i++)

(

doubletemp[5];

fscanf(fp.input,〃%*酰1f%lf%lf%lf%lf〃，

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

，，，，

fprintf(fp_out,%3d\t%101f\t%101f\t%10lf\t%101f\t%101f\n)

i+1,teiup[0],leuiptl],temp[2],leiup[3],leiup[4]);

fclose(fpinput);

fprintf(fpout,〃**************************************

•Jx^r*^T»^«rL*«^r*''

^CX^Z^C^C%^Z>^Z*^XX^Z[[)•

fprintf(fpout,“计算结果如下:\n\n外方位元素：\n〃);

fprintf(fpout.,，/\t.Xs=%101f\n/z,Xs);

fprintf(fp_out,/z\tYs=%10lf\nz，,Ys);

fprintf(fp_out,/z\tZs=%101f\n/z,Zs);

fprintf(fp_out,//\tPhi=%101f\n，z,Phi);

fprintf(fpout,”\tOmega二$101f\n〃，Omega);

fprintf(fp_out,〃\tKappa二$101f\n\n”,Kappa);

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

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

(

fprintf(fp_out,〃\t〃);

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

{

fprintf(fp_out,z，%101f\tzz,*(R+i*3+j));

)

fprintf(fp_out,//\n，/);

)

fprintf(fpout,〃\n单位权中误差:%101f\n\n，，»mO);

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

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

(

fprintf(fp_out,，，\tz，);

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

(

fprintf(fp_out,〃％2Olf\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(fpout,〃\n〃);

fprintf(fp_cut,〃**************************************"

«X*

*T**T**1^*T**T**T^*7**T^*T**7**T**T**T**j**?**T^*T**T**T**j**T*

5^C^|c^|c5jc5^C^|c^|c5^C5|C^|C^|C^|C5|C5|c^|C!^C5|c5|C5|C^C5^C5|C5|C5jC5^C5|c5|C5|C^|C5|C^|C5^C5^C

*1^«>l««X»\、•

*T**«**r**T^*T*\II

fclose(fp_out);

return0;

}

;*1*<U«J*«！««1«*1**1*«1«*1*»1**1**1**1**1«<U«J**1*

•»^•»*

*函数名：Resection

*函数介绍：计算

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

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

*vers

*完成时间：09-10-4

^P*2^*J**1**J**1**J*»j*^1**2^*J»*j**J**|«*J**J*/

intResection(constint&Num,constDOUBLE*&Data,constdouble&m,

constdouble&f)

doubleXs=0,Ys=0,Zs=0;

inti,j;

〃设置初始值

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

(

Xs+=Data[i][2];

Ys+=Data[i][3];

}

Xs/=Num;

Ys/=Num;

Zs=m*f;

doublePhi(0),Omega(0),Kappa(0);

doubleR[3][3]={0.0}:

double*L=nowdouble[2*Num];

typedefdoubleDouble6[6];

Double6*A=newDouble6[2*Num];

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

double*ATA=newdouble[6*6];

double*ATL=newdouble[6];

double*Xg=newdouble[6];

〃迭代计算

do

〃旋转矩阵

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

R[O][l]=-cos(Phi)*sin(Kappa)-sin(Phi)*sin(Omega)*cos(Kappa);

R[O][2]=-sin(Phi)*cos(Omega);

R[l][O]=cos(Omega)*sin(Kappa);

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

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

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

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

R[2][2]=cos(Phi)*cos(Omega);

for(i=O;i<Num;i++)

(

doubleX=R[O][0]*(Data[i][2]-Xs)4-R[l][0]*(Data[i][3]-Ys)+

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

doubleY=R[O][l]*(Data[i][2]-Xs)+R[l][l]*(Data[i][3]-Ys)+

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

doubleZ=R[O][2]*(Data[i][2]Xs)iR[l][2]*(Data[i][3]Ys)।

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

doublexxx,yyy;

xxx=-f*X/Z;

yyy=-f*Y/Z;

〃常数项

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

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

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

A[2*i][l]=(R[l][0]*f+R[l][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+l][0]=(R[0][1]*f+R[0][2]*(yyy))/Z;

A[2*i4-l][l]=(R[l][l]*f+R[l][2]*(yyy))/Z;

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

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

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

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

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

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

A[2*i+l][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))

return1;

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

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

Xs+=Xg[0];

Ys+=Xg[l];

Zs+=Xg[2];

Phi+=Xg[3];

0mega+=Xg[4];

Kappa+=Xg[5];

}while(fabs(Xg[0])^PRECISION||fabs(Xg[l])"PRECISION||

fabs(Xg[2])>=PRECIS1ON||fabs(Xg[3])>=PRECISION||

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

〃注：协因数阵，旋转矩阵等计算本应该使用最后外方位元素值，

〃由于变换很小忽略

double*Q=ATA;

double*V=newdouble[2*Num];

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

doubleVTV=0;

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

(

V[i]-=L[i];

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

}

doublen)0=sqrt(VTV/(2*Num-6));

double*mm=newdouble[6];

fui-(i=0;i<6;i++)

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

}

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

delete[]L;

delete[]A;

delete[]AT;

delete[]ATA;

delete[]ATL;

delete[]Xg;

delete[]mm;

delete[]V;

return0;

)

voidswap(double&a,double&b)

(

doubletemp=a;

a=b;

b=temp;

}

;*1**1**1*

•»*•»*

*函数名：InverseMatrix

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

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

*（in）row（矩阵阶数）

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

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

*调用函数：swap（double&,doubled）

*vcrs

*完成时间:09-10-4

X1Z«£*•]»*1*xl*

,「*Y**Y*，卜^7**,**Y**Y*f*J**Y*，卜。、4、*T**Y*f*Y**T»，&、f*?**Y**T*，&、*T**T»，卜*Y**T*，,、*T**T**Y*，卜

intInverseMatrix(double*matrix,constint&row)

double*m=newdouble[row*row];

double*ptemp,*pt=m;

inti,j;

ptemp=matrix;

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

(

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

(

*pl=+plciiip;

ptemp++;

pt++:

}

intk;

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

for(k=0;k<row;k++)

(

doublemax=0;

〃全选主元

〃寻找最大元素

for(i=k;i<row;i++)

(

for(j=k;j<row;J+4-)

(

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

(

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

is[k]=i;

js[k]=j;

)

)

)

if(0=