摄影测量程序汇总后方交会+前方交会+单模型光束法平差Word下载.docx
《摄影测量程序汇总后方交会+前方交会+单模型光束法平差Word下载.docx》由会员分享,可在线阅读,更多相关《摄影测量程序汇总后方交会+前方交会+单模型光束法平差Word下载.docx(28页珍藏版)》请在冰豆网上搜索。
iostream>
double*readdata();
voidsavedata(inthang,double*data,double*xishuarray,double*faxishu,double*l,inti,doublexs,doubleys,doublezs,doublefai,doubleoumiga,doublekapa);
voidtranspose(double*m1,double*m2,intm,intn);
voidinverse(double*a,intn);
voidmulti(double*mat1,double*mat2,double*result,inta,intb,intc);
voidinverse(double*a,intn)/*正定矩阵求逆*/
{
inti,j,k;
for(k=0;
k<
n;
k++)
{
for(i=0;
i<
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(j=0;
j<
j++)
{
if(j!
*(a+i*n+j)+=*(a+k*n+j)**(a+i*n+k);
}
}
for(j=0;
if(j!
*(a+k*n+j)*=*(a+k*n+k);
}
}
voidtranspose(double*m1,double*m2,intm,intn)//矩阵转置
{inti,j;
for(i=0;
m;
for(j=0;
m2[j*m+i]=m1[i*n+j];
return;
voidmulti(double*mat1,double*mat2,double*result,inta,intb,intc)
{inti,j,k;
for(i=0;
a;
{for(j=0;
c;
{result[i*c+j]=0;
b;
result[i*c+j]+=mat1[i*b+k]*mat2[k*c+j];
return;
double*readdata()
{
FILE*fp;
inti,j;
intnumber;
chardatacatolog[100];
//scanf("
%s"
datacatolog);
if((fp=fopen("
控制点坐标.txt"
"
r"
))==NULL)
printf("
读取数据出错!
\n"
);
returnfalse;
fscanf(fp,"
%d"
&
number);
double*cordata=newdouble[number*5];
for(i=0;
number;
for(j=0;
5;
fscanf(fp,"
%lf"
cordata+i*5+j);
printf("
控制点坐标数据读取成功!
returncordata;
voidsavedata(inthang,double*data,double*xishuarray,double*faxishu,double*l,inti,doublexs,doubleys,doublezs,doublefai,doubleoumiga,doublekapa)
char*file1="
结算数据.txt"
;
fp=fopen(file1,"
w"
fprintf(fp,"
---------原始坐标数据为--------:
for(inti=0;
hang;
for(intj=0;
fprintf(fp,"
%7.4lf"
data[i*5+j]);
fprintf(fp,"
--------------------------------\n"
---------误差方程系数阵为:
--------:
hang*2;
6;
xishuarray[i*5+j]);
---------法方程系数阵为:
%7.5lf"
faxishu[i*5+j]);
---------误差方程常数项为:
%lf"
l[i]);
---------迭代次数为:
%d\n"
i);
-----------外方位元素为:
---------\n"
Xs=%lf,Ys=%lf,Zs=%lf\n"
xs,ys,zs);
fai=%lf,oumiga=%lf,kapa=%lf\n"
fai,oumiga,kapa);
fclose(fp);
voidmain()
intii,jj;
intdiedainumber=0;
doublex0=0.0,y0=0.0,f=0.0;
doublem=50000;
//估算比例尺
doublefai=0,oumiga=0,kapa=0,Xs=0,Ys=0,Zs=0;
doubleR[3][3]={0.0};
doubleX=0.0,Y=0.0,Z=0.0,L[8][1]={0.0},A[8][6]={0.0};
doublecorrect[6][1]={0.0},AT[6][8]={0.0},ATA[6][6]={0.0},ATL[6][1]={0.0};
introw;
//row用于存放坐标行数
double*controlpoint;
controlpoint=readdata();
row=sizeof(controlpoint);
row;
printf("
%3.3lf"
controlpoint[i*5+j]);
/*----------内方位元素----------*/
请输入像片的内方位元素(mm):
x0="
x0/=1000.0;
scanf("
x0);
//double类型数据要用%lf
y0="
y0/=1000.0;
y0);
f="
f);
f=f/1000.0;
/*------------------------------*/
//-------确定未知数初始值------
for(inti=0;
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(inti=0,k=0,j=0;
=3;
i++,k++,j++)
X=R[0][0]*(controlpoint[i*5+2]-Xs)+R[1][0]*(controlpoint[i*5+3]-Ys)+R[2][0]*(controlpoint[i*5+4]-Zs);
Y=R[0][1]*(controlpoint[i*5+2]-Xs)+R[1][1]*(controlpoint[i*5+3]-Ys)+R[2][1]*(controlpoint[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)*((controlpoint[i*5+0]-x0)*cos(kapa)-(controlpoint[i*5+1]-y0)*sin(kapa))/f+f*cos(kapa))*cos(oumiga);
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)*((controlpoint[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(kapa)+(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);
迭代次数为:
diedainumber);
---------误差方程系数为:
--------\n"
8;
%4.4lf"
A[i][j]);
求解得到的外方位元素为:
Xs=%lf\n"
Xs);
Ys=%lf\n"
Ys);
Zs=%lf\n"
Zs);
fai=%lf\n"
fai);
oumiga=%lf\n"
oumiga);
kapa=%lf\n"
kapa);
savedata(row,controlpoint,A[0],ATA[0],L[0],diedainumber,Xs,Ys,Zs,fai,oumiga,kapa);
-----------------解算结束!
--------------\n"
system("
pause"
解算结果:
2.后方交会-前方交会求解地面点坐标
已知左右像片外方位元素,给出像点坐标:
左像点坐标:
右像点坐标:
x(/m)
y(/m)
0.0053
0.0069
0.00482
0.0027
0.00556
0.00229
0.00521
-0.00191
0.00815
0.00499
0.00773
0.00089
-0.00247
0.00156
-0.00279
-0.00292
C语言代码:
voidsavedata(inthang,double*data);
charleftdata[300];
像点坐标数据.txt"
system("
exit(0);
double*c=newdouble[number*4];
for(k=0;
2;
fread(&
leftdata,14,1,fp);
for(i=0;
{
for(j=0;
fscanf(fp,"
c+k*2+i*4+j);
returnc;
voidsavedata(inthang,double*data)
地面点坐标数据.txt"
---------像点对应地面点坐标为--------:
第%d点:
"
i+1);
3;
data[i*3+j]);
\n\n"
-----------------------------------------"
double*imagepoint;
imagepoint=readdata();
row=sizeof(imagepoint);
//--------------------------------------------
doublef=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("
请输入左像片的外方位元素:
//printf("
Xs1="
Xs1);
Ys1="
Ys1);
Zs1="
Zs1);
fai1="
fai1);
oumiga1="
oumiga1);
kapa1="
kapa1);
请输入右像片的外方位元素:
Xs2="
Xs2);
Ys2="
Ys2);
Zs2="
Zs2);
fai2="
fai2);
oumiga2="
oumiga2);
kapa2="
kapa2);
doubleBx=Xs2-Xs1,By=Ys2-Ys1,Bz=Zs2-Zs1;
doubleN1=0,N2=0;
doubleX1=0,Y1=0,Z1=0,X2=0,Y2=0,Z2=0;
doubleR1[3][3]={0.0};
doubleR2[3][3]={0.0};
doubleGEOdata[4][3]={0.0};
//---