摄影测量程序汇总后方交会+前方交会+单模型光束法平差.docx
《摄影测量程序汇总后方交会+前方交会+单模型光束法平差.docx》由会员分享，可在线阅读，更多相关《摄影测量程序汇总后方交会+前方交会+单模型光束法平差.docx（34页珍藏版）》请在冰点文库上搜索。
摄影测量程序汇总后方交会+前方交会+单模型光束法平差
程序运行环境为VisualStudio2010.运行前请先将坐标数据放在debug下。
1.单像空间后方交会
原始数据：
内方位元素
x0（/mm）
y0（/mm）
主距f（/mm）
比例尺分母
0
0
153.24
50000
像点坐标（/mm）
物点坐标（/m）
-86.15
-68.99
36589.41
25273.32
2195.17
-53.4
82.21
37631.08
31324.51
728.69
-14.78
-76.63
39100.97
24934.98
2386.5
10.46
64.43
40426.54
30319.81
757.31
C语言程序：
#include
#include
#include
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{
for（i=0;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{
if（i!
=k）
{
for（j=0;j{
if（j!
=k）
*（a+i*n+j）+=*（a+k*n+j）**（a+i*n+k）;
}
}
}
for（j=0;j{
if（j!
=k）
*（a+k*n+j）*=*（a+k*n+k）;
}
}
}
voidtranspose（double*m1,double*m2,intm,intn）//矩阵转置
{inti,j;
for（i=0;ifor（j=0;jm2[j*m+i]=m1[i*n+j];
return;
}
voidmulti（double*mat1,double*mat2,double*result,inta,intb,intc）
{inti,j,k;
for（i=0;i{for（j=0;j{result[i*c+j]=0;
for（k=0;kresult[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;i{
for（j=0;j<5;j++）
{
fscanf（fp,"%lf",cordata+i*5+j）;
}
}
printf（"控制点坐标数据读取成功！
\n"）;
returncordata;
}
voidsavedata（inthang,double*data,double*xishuarray,double*faxishu,double*l,inti,doublexs,doubleys,doublezs,doublefai,doubleoumiga,doublekapa）
{
FILE*fp;
char*file1="结算数据.txt";
fp=fopen（file1,"w"）;
fprintf（fp,"---------原始坐标数据为--------：
\n"）;
for（inti=0;i{
for（intj=0;j<5;j++）
{
fprintf（fp,"%7.4lf",data[i*5+j]）;
}
fprintf（fp,"\n"）;
}
fprintf（fp,"--------------------------------\n"）;
fprintf（fp,"---------误差方程系数阵为：
--------：
\n"）;
for（inti=0;i{
for（intj=0;j<6;j++）
{
fprintf（fp,"%7.4lf",xishuarray[i*5+j]）;
}
fprintf（fp,"\n"）;
}
fprintf（fp,"--------------------------------\n"）;
fprintf（fp,"---------法方程系数阵为：
--------：
\n"）;
for（inti=0;i<6;i++）
{
for（intj=0;j<6;j++）
{
fprintf（fp,"%7.5lf",faxishu[i*5+j]）;
}
fprintf（fp,"\n"）;
}
fprintf（fp,"--------------------------------\n"）;
fprintf（fp,"---------误差方程常数项为:
--------：
\n"）;
for（inti=0;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;
}
voidmain（）
{
inti,j;
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）;
for（i=0;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类型数据要用%lf
printf（"y0="）;
y0/=1000.0;
scanf（"%lf",&y0）;
printf（"f="）;
scanf（"%lf",&f）;
f=f/1000.0;
/*------------------------------*/
//-------确定未知数初始值------
for（inti=0;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（inti=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]*（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）;
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,kapa）;
printf（"-----------------解算结束！
--------------\n"）;
system（"pause"）;
}
解算结果：
2.后方交会-前方交会求解地面点坐标
已知左右像片外方位元素，给出像点坐标：
左像点坐标：
右像点坐标：
x（/m）
y（/m）
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语言代码：
#include
#include
#include
double*readdata（）;
voidsavedata（inthang,double*data）;
double*readdata（）
{
FILE*fp;
inti,j,k;
intnumber;
chardatacatolog[100];
charleftdata[300];
//scanf（"%s",datacatolog）;
if（（fp=fopen（"像点坐标数据.txt","r"））==NULL）
{
printf（"读取数据出错!
\n"）;
system（"pause"）;
exit（0）;
}
fscanf（fp,"%d",&number）;
double*c=newdouble[number*4];
for（k=0;k<2;k++）
{
fread（&leftdata,14,1,fp）;
for（i=0;i{
for（j=0;j<2;j++）
{
fscanf（fp,"%lf",c+k*2+i*4+j）;
}
}
}
fclose（fp）;
returnc;
}
voidsavedata（inthang,double*data）
{
FILE*fp;
char*file1="地面点坐标数据.txt";
fp=fopen（file1,"w"）;
fprintf（fp,"---------像点对应地面点坐标为--------：
\n"）;
fprintf（fp,"\n"）;
for（inti=0;i{
fprintf（fp,"第%d点：
",i+1）;
for（intj=0;j<3;j++）
{
fprintf（fp,"%7.4lf",data[i*3+j]）;
}
fprintf（fp,"\n\n"）;
}
fprintf（fp,"-----------------------------------------"）;
fclose（fp）;
return;
}
voidmain（）
{
double*imagepoint;
introw;
inti,j;
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（"请输入左像片的外方位元素：
\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）;
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};
for（i=0;i{
//-
升级会员 