sheyingceliang.cpp

摄影测量空间后方交会的源代码

#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);
}