C语言空间后方交会源代码.docx
《C语言空间后方交会源代码.docx》由会员分享,可在线阅读,更多相关《C语言空间后方交会源代码.docx(16页珍藏版)》请在冰豆网上搜索。
![C语言空间后方交会源代码.docx](https://file1.bdocx.com/fileroot1/2023-2/28/0c781836-73bb-41ea-9832-625bc222cb09/0c781836-73bb-41ea-9832-625bc222cb091.gif)
C语言空间后方交会源代码
#include
#include
#definen4//控制点个数
#definePI3.14159265
structcoordinate
{
doublex;//像点坐标
doubley;
doubleXt;//控制点坐标
doubleYt;
doubleZt;
};
//voidinverse(doublec[6][6])//矩阵求逆
//{
//inti,j,h,k;
//doublep;
//doubleq[6][12];
//for(i=0;i<6;i++)//构造高斯矩阵
//for(j=0;j<6;j++)
//q[i][j]=c[i][j];
//for(i=0;i<6;i++)
//for(j=6;j<12;j++)
//{
//if(i+6==j)
//q[i][j]=1;
//else
//q[i][j]=0;
//}
//for(h=k=0;k//for(i=k+1;i<6;i++)
//{
//if(q[i][h]==0)
//continue;
//p=q[k][h]/q[i][h];
////p=q[i][h]/q[k][h];
//for(j=0;j<12;j++)
//{
//q[i][j]*=p;
//q[i][j]-=q[k][j];
//}
//}
//for(h=k=5;k>0;k--,h--)//消去对角线以上的数据
//for(i=k-1;i>=0;i--)
//{
//if(q[i][h]==0)
//continue;
//p=q[k][h]/q[i][h];
////p=q[i][h]/q[k][h];
//for(j=11;j>0;j--)
//{
//q[i][j]*=p;
//q[i][j]-=q[k][j];
//}
//}
//for(i=0;i<6;i++)//将对角线上数据化为1
//{
//p=1.0/q[i][i];
//for(j=0;j<12;j++)
//q[i][j]*=p;
//}
//for(i=0;i<6;i++)//提取逆矩阵
//for(j=0;j//{
//c[i][j]=q[i][j+6];
//}
//}
voidContraryMatrix(double*constpMatrix,double*const_pMatrix,constint&dim)
{
intii,jj,kk;
intflag=0;
double*tMatrix=newdouble[2*dim*dim];
for(inti=0;ifor(intj=0;jtMatrix[i*dim*2+j]=pMatrix[i*dim+j];
}
for(i=0;ifor(intj=dim;jtMatrix[i*dim*2+j]=0.0;
tMatrix[i*dim*2+dim+i]=1.0;
}
//Initializationover!
for(i=0;i{
doublebase=tMatrix[i*dim*2+i];
if(fabs(base)<1E-300)
{
for(ii=i;ii{
if(tMatrix[ii*dim*2+i]!
=0)
{
flag=1;
for(jj=0;jj<2*dim;jj++)
{
tMatrix[i*dim*2+jj]+=tMatrix[ii*dim*2+jj];
}
}
}
base=tMatrix[i*dim*2+i];
if(flag==0)
{
printf("求逆矩阵过程中被零除,无法求解!
");
}
//exit(0);
}
for(intj=0;j{
if(j==i)continue;
doubletimes=tMatrix[j*dim*2+i]/base;
for(intk=0;k{
tMatrix[j*dim*2+k]=tMatrix[j*dim*2+k]-times*tMatrix[i*dim*2+k];
}
}
for(intk=0;ktMatrix[i*dim*2+k]/=base;
}
}
for(i=0;i{
for(intj=0;j_pMatrix[i*dim+j]=tMatrix[i*dim*2+j+dim];
}
delete[]tMatrix;
}
voidmain()
{
doublef,xo,yo;//内方位元素
intm;//摄影比例尺分母
f=0.15324;
xo=0;yo=0;
m=50000;
structcoordinatecoor[n]={{-0.08615,-0.06899,36589.41,25273.32,2195.17},{-0.05340,0.08221,37631.08,31324.51,728.69},{-0.01478,-0.07663,39100.97,24934.98,2386.50},
{0.01046,0.06443,40426.54,30319.81,757.31}};
inti,j;//循环变量
doubleXs,Ys,Zs;//外方位线元素
doublephi,omega,kappa;//外方位角元素
doubleR[3][3];//旋转矩阵R
double(x)[n],(y)[n];//控制点像点坐标的近似值
doubleA[8][6];//误差方程中的矩阵A
doubleATA[6][6];//矩阵A的转置矩阵与A的乘积
doubleL[8][1];
doubleATL[6][1];//矩阵A的转置矩阵与L的乘积
double_ATA[6][6];
doubleX[6][1];//未知数矩阵
doubleV[8][1];//改正数矩阵
doubleDXs,DYs,DZs,Dphi,Domega,Dkappa;//6个外方位元素的改正值
//与精度计算有关的量
doublem0;//单位权中误差
doubleQ[6][6];//协方差矩阵
doublem1,m2,m3,m4,m5,m6;//未知数Xs,Ys,Zs,phi,omega,kappa的中误差矩阵
Xs=0;Ys=0;
for(i=0;i{
Xs+=coor[i].Xt;
Ys+=coor[i].Yt;
}
//外方位元素的初始值
Xs/=n;
Ys/=n;
Zs=f*m;
phi=0;
omega=0;
kappa=0;//在竖直摄影情况下
do
{
//计算旋转矩阵R的各个元素
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);
//将未知数的近似值代人共线方程式,计算(x),(y)
for(i=0;i{
(x)[i]=-f*(R[0][0]*(coor[i].Xt-Xs)+R[1][0]*(coor[i].Yt-Ys)+R[2][0]*(coor[i].Zt-Zs))/(R[0][2]*(coor[i].Xt-Xs)+R[1][2]*(coor[i].Yt-Ys)+R[2][2]*(coor[i].Zt-Zs));
(y)[i]=-f*(R[0][1]*(coor[i].Xt-Xs)+R[1][1]*(coor[i].Yt-Ys)+R[2][1]*(coor[i].Zt-Zs))/(R[0][2]*(coor[i].Xt-Xs)+R[1][2]*(coor[i].Yt-Ys)+R[2][2]*(coor[i].Zt-Zs));
}
//计算矩阵A的各个元素
for(i=0;i{
A[2*i][0]=(R[0][0]*f+R[0][2]*(coor[i].x-xo))/(R[0][2]*(coor[i].Xt-Xs)+R[1][2]*(coor[i].Yt-Ys)+R[2][2]*(coor[i].Zt-Zs));
A[2*i][1]=(R[1][0]*f+R[1][2]*(coor[i].x-xo))/(R[0][2]*(coor[i].Xt-Xs)+R[1][2]*(coor[i].Yt-Ys)+R[2][2]*(coor[i].Zt-Zs));
A[2*i][2]=(R[2][0]*f+R[2][2]*(coor[i].x-xo))/(R[0][2]*(coor[i].Xt-Xs)+R[1][2]*(coor[i].Yt-Ys)+R[2][2]*(coor[i].Zt-Zs));
A[2*i][3]=(coor[i].y-yo)*sin(omega)-((coor[i].x-xo)/f*((coor[i].x-xo)*cos(kappa)-(coor[i].y-yo)*sin(kappa))+f*cos(kappa))*cos(omega);
A[2*i][4]=-f*sin(kappa)-(coor[i].x-xo)/f*((coor[i].x-xo)*sin(kappa)+(coor[i].y-yo)*cos(kappa));
A[2*i][5]=coor[i].y-yo;
A[2*i+1][0]=(R[0][1]*f+R[0][2]*(coor[i].y-yo))/(R[0][2]*(coor[i].Xt-Xs)+R[1][2]*(coor[i].Yt-Ys)+R[2][2]*(coor[i].Zt-Zs));
A[2*i+1][1]=(R[1][1]*f+R[1][2]*(coor[i].y-yo))/(R[0][2]*(coor[i].Xt-Xs)+R[1][2]*(coor[i].Yt-Ys)+R[2][2]*(coor[i].Zt-Zs));
A[2*i+1][2]=(R[2][1]*f+R[2][2]*(coor[i].y-yo))/(R[0][2]*(coor[i].Xt-Xs)+R[1][2]*(coor[i].Yt-Ys)+R[2][2]*(coor[i].Zt-Zs));
A[2*i+1][3]=-(coor[i].x-xo)*sin(omega)-((coor[i].y-yo)/f*((coor[i].x-xo)*cos(kappa)-(coor[i].y-yo)*sin(kappa))-f*sin(kappa))*cos(omega);
A[2*i+1][4]=-f*cos(kappa)-(coor[i].y-yo)/f*((coor[i].x-xo)*sin(kappa)+(coor[i].y-yo)*cos(kappa));
A[2*i+1][5]=-(coor[i].x-xo);
}
for(i=0;i<6;i++)
{
for(j=0;j<6;j++)
{
printf("%f",A[i][j]);
if(j%5==0&&j!
=0)
{
printf("\n");
}
}
}
printf("'\n");
//计算ATA的各个元素
for(i=0;i<6;i++)
for(j=0;j<6;j++)
ATA[i][j]=A[0][i]*A[0][j]+A[1][i]*A[1][j]+A[2][i]*A[2][j]+A[3][i]*A[3][j]+A[4][i]*A[4][j]
+A[5][i]*A[5][j]+A[6][i]*A[6][j]+A[7][i]*A[7][j];
for(i=0;i<6;i++)
{
for(j=0;j<6;j++)
{
_ATA[i][j]=ATA[i][j];
printf("%f",ATA[i][j]);
if(j%5==0&&j!
=0)
{
printf("\n");
}
}
}
printf("\n");
//计算矩阵L的各个元素
for(i=0;i{
L[2*i][0]=coor[i].x+f*(R[0][0]*(coor[i].Xt-Xs)+R[1][0]*(coor[i].Yt-Ys)+R[2][0]*(coor[i].Zt-Zs))/(R[0][2]*(coor[i].Xt-Xs)+R[1][2]*(coor[i].Yt-Ys)+R[2][2]*(coor[i].Zt-Zs));
L[2*i+1][0]=coor[i].y+f*(R[0][1]*(coor[i].Xt-Xs)+R[1][1]*(coor[i].Yt-Ys)+R[2][1]*(coor[i].Zt-Zs))/(R[0][2]*(coor[i].Xt-Xs)+R[1][2]*(coor[i].Yt-Ys)+R[2][2]*(coor[i].Zt-Zs));
}
//计算矩阵ATL的各个元素值
for(i=0;i<6;i++)
ATL[i][0]=A[0][i]*L[0][0]+A[1][i]*L[1][0]+A[2][i]*L[2][0]+A[3][i]*L[3][0]+A[4][i]*L[4][0]
+A[5][i]*L[5][0]+A[6][i]*L[6][0]+A[7][i]*L[7][0];
/*for(i=0;i<6;i++)
{
for(j=0;j<1;j++)
{
ATL[i][j]=0;
for(intk=0;k<8;k++)
{
ATL[i][j]+=A[k][i]*L[k][j];
}
}
}*/
//调用函数计算矩阵ATA的逆矩阵
//inverse(ATA);
ContraryMatrix(*_ATA,*ATA,6);
for(i=0;i<6;i++)
{
for(j=0;j<6;j++)
{
printf("%f",ATA[i][j]);
if(j%5==0&&j!
=0)
{
printf("\n");
}
}
}
//计算矩阵X的各个元素值
for(i=0;i<6;i++)
{
X[i][0]=ATA[i][0]*ATL[0][0]+ATA[i][1]*ATL[1][0]+ATA[i][2]*ATL[2][0]+ATA[i][3]*ATL[3][0]
+ATA[i][4]*ATL[4][0]+ATA[i][5]*ATL[5][0];
printf("%f",X[i][0]);
}
//for(i=0;i<6;i++)
//{
//for(j=0;j<1;j++)
//{
//X[i][j]=0;
//for(intk=0;k<6;k++)
//{
//X[i][j]+=ATA[i][k]*ATL[k][j];
//
//}
//printf("%f",X[i][j]);
//}
//}
DXs=X[0][0];
DYs=X[1][0];
DZs=X[2][0];
Dphi=X[3][0];
Domega=X[4][0];
Dkappa=X[5][0];
Xs+=DXs;Ys+=DYs;Zs+=DZs;phi+=Dphi;omega+=Domega;kappa+=Dkappa;
//计算矩阵V的各个元素
for(i=0;i{
V[2*i][0]=A[2*i][0]*DXs+A[2*i][1]*DYs+A[2*i][2]*DZs+A[2*i][3]*Dphi+A[2*i][4]*Domega+A[2*i][5]*Dkappa-L[2*i][0];
V[2*i+1][0]=A[2*i+1][0]*DXs+A[2*i+1][1]*DYs+A[2*i+1][2]*DZs+A[2*i+1][3]*Dphi+A[2*i+1][4]*Domega+A[2*i+1][5]*Dkappa-L[2*i+1][0];
}
/*//像点坐标改正后的值
for(i=0;i{
coor[i].x+=V[2*i][0];
coor[i].y+=V[2*i+1][0];
}*/
}
//判断限差的条件
while(!
(fabs(Dphi)<0.1/60/180*PI&&fabs(Domega)<0.1/60/180*PI&&fabs(Dkappa)<0.1/60/180*PI));
//外方位元素计算完毕
//有关精度的计算
for(i=0;i<6;i++)
Q[i][i]=ATA[i][i];
m0=sqrt((V[0][0]*V[0][0]+V[1][0]*V[1][0]+V[2][0]*V[2][0]+V[3][0]*V[3][0]+V[4][0]*V[4][0]+V[5][0]*V[5][0]+V[6][0]*V[6][0]+V[7][0]*V[7][0])/(2*n-6));
//计算各未知数的中误差
m1=sqrt(Q[0][0])*m0;
m2=sqrt(Q[1][1])*m0;
m3=sqrt(Q[2][2])*m0;
m4=sqrt(Q[3][3])*m0;
m5=sqrt(Q[4][4])*m0;
m6=sqrt(Q[5][5])*m0;
printf("改正后的相对坐标及其对应的地面点坐标(x,y,Xt,Yt,Zt):
\n");
for(i=0;i{
printf("%lf\t%lf\t%lf\t%lf\t%lf",(x)[i],(y)[i],coor[i].Xt,coor[i].Yt,coor[i].Zt);
printf("\n");
}
printf("旋转矩阵R:
\n");
for(i=0;i<3;i++)
{
for(j=0;j<3;j++)
printf("%lf\t",R[i][j]);
printf("\n");
}
printf("外方位元素(Xs,Ys,Zs,phi,omega,kappa)及改正值:
\n");
printf("%lf+-%lf\n%lf+-%lf\n%lf+-%lf\n%lf+-%lf\n%lf+-%lf\n%lf+-%lf\n",Xs,DXs,Ys,DYs,Zs,DZs,phi,Dphi,omega,Domega,kappa,Dkappa);
printf("单位权中误差:
\n");
printf("%0.9f\n",m0);
printf("外方位元素(Xs,Ys,Zs,phi,omega,kappa)的中误差:
\n");
printf("%lf\n%lf\n%lf\n%lf\n%lf\n%lf\n",m1,m2,m3,m4,m5,m6);
//计算完毕,输出结果,以文件方式保存
printf("计算结果存储在文件中\n");
FILE*fp;
fp=fopen("计算结果.txt","w");
fprintf(fp,"改正后的相对坐标及其对应的地面点坐标(x,y,Xt,Yt,Zt):
\n");
for(i=0;i{
fprintf(fp,"%lf\t%lf\t%lf\t%lf\t%lf",coor[i].x,coor[i].y,coor[i].Xt,coor[i]