C语言空间后方交会源代码.docx
《C语言空间后方交会源代码.docx》由会员分享，可在线阅读，更多相关《C语言空间后方交会源代码.docx（16页珍藏版）》请在冰豆网上搜索。
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]
升级会员 