#include<iostream>
#include<iomanip>
#include<cmath>
#define pi 3.14159265
using namespace std;
int invers_matrix(double *m1,int n);
void mult(double *m1,double *m2,double *result,int m,int p,int n);
int main()
{double A[8][6],Az[6][8],X[6][1],L[8][1],AA[6][6],AAz[6][8],precision[6],V[8][1];
double f,x0,y0,x_,y_,p,w,k,Xs,Ys,Zs,a1,a2,a3,b1,b2,b3,c1,c2,c3,X0,Y0,Z0,v=0,m;
int M,i,j,s,c=0,num=0,number;
double B[4][5]={-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};
cout<<"××××××××欢迎来到空间后方交会计算程序××××××××"<<endl;
cout<<"请选择服务项目:"<<endl<<"1.使用默认值进行运算"<<endl<<endl<<"2.自行输入数据进行运算"<<endl<<endl;
cin>>number;
cout<<endl<<endl;
while(number!=1&&number!=2)
{cout<<"您的输入有误!请重新选择服务项目:";
cin>>number;
}
if(number==1)
{
f=0.15324,x0=0,y0=0,M=50000;
}
if(number==2)
{
cout<<"请分别输入内方位元素f,x0和y0:"<<endl;
cin>>f>>x0>>y0;
cout<<"请输入比例尺大小:"<<endl<<"1/";
cin>>M;
for(i=0;i<4;i++)
{cout<<"请输入第"<<i+1<<"个点的像片坐标x和y以及地面点坐标Xt,Yt和Zt:"<<endl;
for(j=0;j<5;j++)
cin>>B[i][j];
}
}
Xs=Ys=0.0; //为外方位元素赋初值
for(i=0;i<4;i++)
{Xs+=B[i][2];
Ys+=B[i][3];
}
Xs=Xs/4.0;
Ys=Ys/4.0;
Zs=M*f;
p=w=k=0.0;
for(;;) //迭代的开始,一个大循环
{
a1=cos(p)*cos(k)-sin(p)*sin(w)*sin(k);
a2=-cos(p)*sin(k)-sin(p)*sin(w)*cos(k);
a3=-sin(p)*cos(w);
b1=cos(w)*sin(k);
b2=cos(w)*cos(k);
b3=-sin(w);
c1=sin(p)*cos(k+cos(p)*sin(w)*sin(k));
c2=-sin(p)*sin(k)+cos(p)*sin(w)*cos(k);
c3=cos(p)*cos(w);
for(i=0;i<4;i++)
{
X0=a1*(B[i][2]-Xs)+b1*(B[i][3]-Ys)+c1*(B[i][4]-Zs);
Y0=a2*(B[i][2]-Xs)+b2*(B[i][3]-Ys)+c2*(B[i][4]-Zs);
Z0=a3*(B[i][2]-Xs)+b3*(B[i][3]-Ys)+c3*(B[i][4]-Zs);
A[2*i][0]=(a1*f+a3*(B[i][0]-x0))/Z0;
A[2*i][1]=(b1*f+b3*(B[i][0]-x0))/Z0;
A[2*i][2]=(c1*f+c3*(B[i][0]-x0))/Z0;
A[2*i][3]=(B[i][1]-y0)*sin(w)-(B[i][0]-x0)*(B[i][0]-x0)*cos(k)*cos(w)/f-f*cos(k)*cos(w)+(B[i][0]-x0)*(B[i][1]-y0)*sin(k)*cos(k)/f;
A[2*i][4]=-f*sin(k)-(B[i][0]-x0)*(B[i][0]-x0)*sin(k)/f-(B[i][0]-x0)*(B[i][1]-y0)*cos(k)/f;
A[2*i][5]=B[i][1]-y0;
A[2*i+1][0]=(a2*f+a3*(B[i][1]-y0))/Z0;
A[2*i+1][1]=(b2*f+b3*(B[i][1]-y0))/Z0;
A[2*i+1][2]=(c2*f+c3*(B[i][1]-y0))/Z0;
A[2*i+1][3]=-(B[i][0]-x0)*sin(w)-(B[i][1]-y0)*(B[i][0]-x0)*cos(k)*cos(w)/f+f*sin(k)*cos(w)+(B[i][1]-y0)*(B[i][1]-y0)*sin(k)*cos(w)/f;
A[2*i+1][4]=-f*cos(k)-(B[i][1]-y0)*(B[i][0]-x0)*sin(k)/f-(B[i][1]-y0)*(B[i][1]-y0)*cos(k)/f;
A[2*i+1][5]=x0-B[i][0];
//得到近似值
x_=-f*X0/Z0;
y_=-f*Y0/Z0;
//得到L矩阵
L[2*i][0]=B[i][0]-x_;
L[2*i+1][0]=B[i][1]-y_;
}
//转置
for(i=0;i<8;i++)
for(j=0;j<6;j++)
Az[j][i]=A[i][j];
mult(*Az,*A,*AA,6,8,6);
if(invers_matrix(*AA,6)==NULL)
exit(-1);
mult(*AA,*Az, *AAz,6,6,8);
mult( *AAz, *L, *X,6,8,1);
mult(*A,*X,*V,8,6,1);
Xs=Xs+X[0][0];
Ys=Ys+X[1][0];
Zs=Zs+X[2][0];
p=p+X[3][0];
w=w+X[4][0];
k=k+X[5][0];
num++; //迭带次数增加
s=0;
for(i=0;i<3;i++)
{if(X[i][0]>0.01)
s=1;}
for(i=3;i<6;i++)
{ if(X[i][0]>2*pi/(360*60*60))
s=1;}
if(s==0) break;
}
//单位弧度化角度
p=p*360*60/(2*pi);
w=w*360*60/(2*pi);
k=k*360*60/(2*pi);
for(i=0;i<8;i++)
V[i][0]-=L[i][0];
for(i=0;i<4;i++)
for(j=0;j<2;j++)
{
B[i][j]+=V[c][0];
c++;
}
for(i=0;i<8;i++)
v+=(V[i][0]*V[i][0]);
m=sqrt(v/(2*4-6)); //单位权中误差
for(i=0;i<6;i++)
precision[i]=m*sqrt(AA[i][i]); //未知数精度
cout<<"单片空间后方交会计算结果:"<<endl<<endl<<endl;
cout<<"像点坐标及对应地面点坐标(单位:米):"<<endl<<endl;
for(i=0;i<4;i++)
{cout<<"第"<<i+1<<"点:";
for(j=0;j<5;j++)
cout<<B[i][j]<<' ';
cout<<endl<<endl; }
cout<<endl<<endl;
cout<<"单位权中误差(单位:微米):"<<"m="<<setprecision(3)<<m*1000000<<endl<<endl<<endl<<endl;
cout<<"外方位元素(单位:米,角度(分)):"<<endl<<endl;
cout<<"Xs="<<setw(10)<<setprecision(6)<<Xs<<endl<<endl;
cout<<"Ys="<<setw(10)<<setprecision(6)<<Ys<<endl<<endl;
cout<<"Zs="<<setw(10)<<setprecision(6)<<Zs<<endl<<endl;
cout<<"p="<<setw(10)<<setprecision(3)<<p<<endl<<endl;
cout<<"w="<<setw(10)<<setprecision(3)<<w<<endl<<endl;
cout<<"k="<<setw(10)<<setprecision(3)<<k<<endl<<endl<<endl<<endl;
cout<<"外方位元素对应精度:"<<endl<<endl;
cout<<"Xs:"<<setw(10)<<precision[0]<<endl<<endl;
cout<<"Ys:"<<setw(10)<<precision[1]<<endl<<endl;
cout<<"Zs:"<<setw(10)<<precision[2]<<endl<<endl;
cout<<"p:"<<setw(10)<<precision[3]<<endl<<endl;
cout<<"w:"<<setw(10)<<precision[4]<<endl<<endl;
cout<<"k:"<<setw(10)<<precision[5]<<endl<<endl<<endl<<endl;
cout<<"迭代次数:"<<num<<endl<<endl<<endl;
system("PAUSE");
return 0;
}
//矩阵相乘
void mult(double *m1,double *m2,double *result,int m,int p,int n)
{
int i,j,k;
for(i=0;i<m;i++)
for(j=0;j<n;j++)
{
result[i*n+j]=0.0;
for(k=0;k<p;k++)
result[i*n+j]+=m1[i*p+k]*m2[j+k*n];
}
return;
}
//矩阵求逆
int invers_matrix(double *m1,int n)
{
int *is,*js;
int i,j,k,l,u,v;
double temp,max_v;
is=(int *)malloc(n*sizeof(int));
js=(int *)malloc(n*sizeof(int));
if(is==NULL||js==NULL)
{
printf("out of memory!\n");
return(0);
}
for(k=0;k<n;k++)
{
max_v=0.0;
for(i=k;i<n;i++)
for(j=k;j<n;j++)
{
temp=fabs(m1[i*n+j]);
if(temp>max_v)
{
max_v=temp; is[k]=i; js[k]=j;
}
}
if(max_v==0.0)
{
free(is); free(js);
printf("invers is not availble!\n");
return(0);
}
if(is[k]!=k)
for(j=0;j<n;j++)
{
u=k*n+j; v=is[k]*n+j;
temp=m1[u]; m1[u]=m1[v]; m1[v]=temp;
}
if(js[k]!=k)
for(i=0;i<n;i++)
{
u=i*n+k; v=i*n+js[k];
temp=m1[u]; m1[u]=m1[v]; m1[v]=temp;
}
l=k*n+k;
m1[l]=1.0/m1[l];
for(j=0;j<n;j++)
if(j!=k)
{
u=k*n+j;
m1[u]*=m1[l];
}
for(i=0;i<n;i++)
if(i!=k)
for(j=0;j<n;j++)
if(j!=k)
{
u=i*n+j;
m1[u]-=m1[i*n+k]*m1[k*n+j];
}
for(i=0;i<n;i++)
if(i!=k)
{
u=i*n+k;
m1[u]*=-m1[l];
}
}
for(k=n-1;k>=0;k--)
{
if(js[k]!=k)
for(j=0;j<n;j++)
{
u=k*n+j; v=js[k]*n+j;
temp=m1[u]; m1[u]=m1[v]; m1[v]=temp;
}
if(is[k]!=k)
for(i=0;i<n;i++)
{
u=i*n+k; v=i*n+is[k];
temp=m1[u]; m1[u]=m1[v]; m1[v]=temp;
}
}
free(is);
free(js);
return(1);
}