后方交会程序实现c语言版.docx
《后方交会程序实现c语言版.docx》由会员分享，可在线阅读，更多相关《后方交会程序实现c语言版.docx（10页珍藏版）》请在冰豆网上搜索。
后方交会程序实现c语言版
#include
#include
#include
#include
#include
#defineN4
#defineT1.41421
voidturn（double*A,doubleA2[],intm,intn）//计算矩阵的转置
{inti,j;
for（i=0;ifor（j=0;jA2[j*m+i]=A[i*n+j];
}
voidmulAB（double*A,double*B,double*C,intam,intan,intbm,intbn）//计算两矩阵相乘
{
inti,j,l,u;
if（an!
=bm）
{
printf（"error!
cannotdothemultiplication.\n"）;
return;
}
for（i=0;ifor（j=0;j{
u=i*bn+j;
C[u]=0.0;
for（l=0;lC[u]+=A[i*an+l]*B[l*bn+j];
}
return;
}
double*inv（double*a,intn）//计算矩阵的逆,本程序的难点,采用高斯约旦全选主元法{
int*is,*js,i,j,k,l,u,v;
doubled,p;
is=（int*）malloc（n*sizeof（int））;
js=（int*）malloc（n*sizeof（int））;
for（k=0;k<=n-1;k++）
{
d=0.0;
for（i=k;ifor（j=k;j{l=i*n+j;
p=fabs（a[l]）;
if（p>d）
{
d=p;
is[k]=i;
js[k]=j;
}
}
if（d+1.0==1.0）
{free（is）;
free（js）;
printf（"errornotinv\n"）;
returnNULL;
}
if（is[k]!
=k）
for（j=0;jv=is[k]*n+j;
p=a[u];
a[u]=a[v];
a[v]=p;
}
if（js[k]!
=k）
for（i=0;iv=i*n+js[k];
p=a[u];
a[u]=a[v];
a[v]=p;
}
l=k*n+k;
a[l]=1.0/a[l];
for（j=0;jif（j!
=k）
{
u=k*n+j;
a[u]=a[u]*a[l];
}
for（i=0;iif（i!
=k）
for（j=0;jif（j!
=k）
{u=i*n+j;
a[u]=a[u]-a[i*n+k]*a[k*n+j];
}
for（i=0;iif（i!
=k）
{u=i*n+k;
a[u]=-a[u]*a[l];
}
}
for（k=n-1;k>=0;k--）
{if（js[k]!
=k）
for（j=0;j<=n-1;j++）
{u=k*n+j;
v=js[k]*n+j;
p=a[u];
a[u]=a[v];
a[v]=p;
}
if（is[k]!
=k）
for（i=0;i{u=i*n+k;
v=i*n+is[k];
p=a[u];
a[u]=a[v];
a[v]=p;
}
}
free（is）;
free（js）;
returna;
}
main（）//主函数,空间后方交会的计算{
FILE*fp;//定义一个文件指针
longm;
inti,j=0,it;
doubleG[1000];
double
f,t,w,k,limit=0,S1=0.0,S2=0.0,S3=0.0,x[N]={0},y[N]={0},x0[N]={0},y0[N]={0},X[N]={0},Y[N]={0},Z[N]={0},Xs0,Ys0,Zs0;
double
a[3],b[3],c[3],A[2*N*6],AT[6*2*N],ATA[6*6],*ATA_=NULL,l[2*N],ATl[6],V[6]={0};
doubleF[6],Qx[6][6],Mi[6][6];
if（（fp=fopen（"e:
\\shuju.txt","r"））==NULL）//使fp指向被打开的shuju.txt文件
{//并判断文件是否打开成功
printf（"\nerroronopenshuju.txt\n"）;
getch（）;
exit
（1）;
}
for（i=0;ifscanf（fp,"%lf%lf%lf%lf%lf",&x[i],&y[i],&X[i],&Y[i],&Z[i]）;//将文件中的数据赋给主函数定义的变量
printf（"原始数据:
\n"）;
printf（"x\t\ty\t\tX\t\tY\t\tZ\t\t\n"）;//输出文件中的原始数据
for（i=0;iprintf（"%.3lf\t\t%.3lf\t\t%.3lf\t%.3lf\t%.3lf\n",x[i],y[i],X[i],Y[i],Z[i]）;
printf（"\n请输入摄影机主距和摄影比例尺分母;f,m:
"）;
scanf（"%lf%ld",&f,&m）;//输入f,m
f=f/1000.0;
for（i=0;i{x[i]/=1000.0;
y[i]/=1000.0;
S1+=X[i];
S2+=Y[i];
S3+=Z[i];
}
Xs0=S1/N;
Ys0=S2/N;
Zs0=m*f+S3/N;
t=0.0;w=0.0;k=0.0;//计算外方位元素的初始值
while
（1）
{
printf（"\n---------------------------------第%d次计算------------------------------\n",j+1）;
a[0]=cos（t）*cos（k）-sin（t）*sin（w）*sin（k）;
a[1]=-cos（t）*sin（k）-sin（t）*sin（w）*cos（k）;
a[2]=-sin（t）*cos（w）;
b[0]=cos（w）*sin（k）;
b[1]=cos（w）*cos（k）;
b[2]=-sin（w）;
c[0]=sin（t）*cos（k）+cos（t）*sin（w）*sin（k）;
c[1]=-sin（t）*sin（k）+cos（t）*sin（w）*cos（k）;
c[2]=cos（t）*cos（w）;//计算旋转矩阵
for（i=0;i{
x0[i]=-f*（a[0]*（X[i]-Xs0）+b[0]*（Y[i]-Ys0）+c[0]*（Z[i]-Zs0））/（a[2]*（X[i]-Xs0）+b[2]*（Y[i]-Ys0）+c[2]*（Z[i]-Zs0））;
y0[i]=-f*（a[1]*（X[i]-Xs0）+b[1]*（Y[i]-Ys0）+c[1]*（Z[i]-Zs0））/（a[2]*（X[i]-Xs0）+b[2]*（Y[i]-Ys0）+c[2]*（Z[i]-Zs0））;
//计算像点坐标近似值
G[i]=a[2]*（X[i]-Xs0）+b[2]*（Y[i]-Ys0）+c[2]*（Z[i]-Zs0）;
}
for（i=0;i{
A[i*12+0]=（a[0]*f+a[2]*x[i]）/G[i];
A[i*12+1]=（b[0]*f+b[2]*x[i]）/G[i];
A[i*12+2]=（c[0]*f+c[2]*x[i]）/G[i];
A[i*12+3]=y[i]*sin（w）-（x[i]*（x[i]*cos（k）-y[i]*sin（k））/f+f*cos（k））*cos（w）;
A[i*12+4]=-f*sin（k）-x[i]*（x[i]*sin（k）+y[i]*cos（k））/f;
A[i*12+5]=y[i];
A[i*12+6]=（a[1]*f+a[2]*y[i]）/G[i];
A[i*12+7]=（b[1]*f+b[2]*y[i]）/G[i];
A[i*12+8]=（c[1]*f+c[2]*y[i]）/G[i];
A[i*12+9]=-x[i]*sin（w）-（y[i]*（x[i]*cos（k）-y[i]*sin（k））/f-f*sin（k））*cos（w）;
A[i*12+10]=-f*cos（k）-y[i]*（x[i]*sin（k）+y[i]*cos（k））/f;
A[i*12+11]=-x[i];
l[i*2+0]=x0[i]-x[i];
l[i*2+1]=y0[i]-y[i];//计算误差方程的系数阵以及lx,ly
}
//printf（"outputmatrix:
A\n"）;
//printmatrix（A,2*N,6）;
//printf（"outputmatrix:
l\n"）;
//printmatrix（l,2*N,1）;
turn（A,AT,2*N,6）;//计算AT
//printf（"outputmatrix:
AT\n"）;
//printmatrix（AT,6,2*N）;
mulAB（AT,A,ATA,6,2*N,2*N,6）;//计算ATA,组法方程
ATA_=inv（ATA,6）;//计算ATA的逆,中间量
intp;
intcnt=-1;
for（it=0;it<36;it++）{
p=it%6;
if（it%6==0）{
cnt++;
}
Qx[cnt][p++]=ATA_[it];
}
for（intit=0;it<6;it++）{
for（intjt=0;jt<6;jt++）{
if（it!
=jt）{
Qx[it][jt]=0;//提取Qx的主对角线元素=Qii
}
//printf（"%-10.3lf",Qx[it][jt]）;
}
//printf（"\n"）;
}
mulAB（AT,l,ATl,6,2*N,2*N,1）;//计算常数项ATL
//printf（"outpitmatrinx:
ATl\n"）;
//printmatrix（ATl,6,1）;
mulAB（ATA_,ATl,V,6,6,6,1）;//解法方程,求改正数,
//printf（"outputmatrix:
V\n"）;
//printmatrix（V,6,1）;
Xs0+=V[0];
Ys0+=V[1];
Zs0+=V[2];
t+=V[3];
w+=V[4];
k+=V[5];
for（i=0;i<6;i++）{
F[i]=V[i]/T;//m0
}
printf（"第%d次计算的外方位元素为:
\n",++j）;
printf（"Xs=%.5lf\tYs=%.5lf\tZs=%.5lf\nt=%.5lf\tw=%.5lf\tk=%.5lf\n",Xs0,Ys0,Zs0,t,w,k）;
if（Xs0-limit<=0.0001&&Xs0-limit>=-0.0001）{//控制迭代次数
break;
}
limit=Xs0;
}
printf（"\n外方位元素为:
\n"）;
printf（"Xs=%.5lf\tYs=%.5lf\tZs=%.5lf\nt=%.5lf\tw=%.5lf\tk=%.5lf\n",Xs0,Ys0,Zs0,t,w,k）;
printf（"\n中误差为:
\n"）;
for（i=0;i<6;i++）{
for（j=0;j<6;j++）{
Mi[i][j]=F[i]*（sqrt（Qx[i][j]））;//mi=m0*Qii开根号
printf（"%-13.10lf",Mi[i][j]）;
}
printf（"\n"）;
}
fclose（fp）;
return0;
}
升级会员 