空间后方交会.cpp

空间后方交会

// 空间后方交会.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include "stdio.h"
#include "math.h"
#include "iostream.h"
#define N 4
#define M 2*N

void Transpose(double a[M][6],double b[6][M])
{
	int i,j;
	for(i=0;i<6;i++)
	  for(j=0;j<M;j++)
	  {
	  	b[i][j]=a[j][i];
	  }
}
void Matrix_Times1(double a[6][M],double b[M][6],double c[6][6])
{	
	int i,j,k;
    for(i=0;i<6;i++)
    for(j=0;j<6;j++)
	{    c[i][j]=0;
		for(k=0;k<2*N;k++)
        c[i][j]+=a[i][k]*b[k][j];
	}
	
}
void Matrix_Times2(double a[6][M],double b[M][1],double c[6][1])
{	
	int i,j,k;
    for(i=0;i<6;i++)
    for(j=0;j<1;j++)
	{    c[i][j]=0;
		for(k=0;k<2*N;k++)
        c[i][j]+=a[i][k]*b[k][j];
	}
	
}
void Matrix_Times3(double a[6][6],double b[6][1],double c[6][1])
{	
	int i,j,k;
    for(i=0;i<6;i++)
    for(j=0;j<1;j++)
	{   c[i][j]=0;
		for(k=0;k<6;k++)
        c[i][j]+=a[i][k]*b[k][j];
	}
	
}
//计算逆矩阵
double js(double s[6][6],int n) 
{
	int z,j,k; 
    double b[6][6],r,total=0;             //b[M][M]用于存放，在矩阵s[N][N]中元素s[0]的余子式 
    if(n>2)
    {
     	for(z=0;z<n;z++) 
     {
    	for(j=0;j<n-1;j++) 
        for(k=0;k<n-1;k++) 
        if(k>=z) b[j][k]=s[j+1][k+1]; 
        else b[j][k]=s[j+1][k]; 
        if(z%2==0) r=s[0][z]*js(b,n-1);       //递归调用
        else  r=(-1)*s[0][z]*js(b,n-1); 
        total=total+r; 
     } 
    } 
    else if(n==2) total=s[0][0]*s[1][1]-s[0][1]*s[1][0]; 
    return total; 
} 
void juzhen_ni(double s[6][6],double b[6][6],int n) 
{
	int z,j,k,l,m,g; 
	double a[6][6],r,temp;
    for(z=0;z<n;z++) 
   {
		l=z; 
    for(j=0;j<n;j++) 
	{
		m=j; 
       for (k=0;k<n-1;k++) 
        for(g=0;g<n-1;g++) 
		{ 
			if(g>=m&&k<l) a[k][g]=s[k][g+1]; 
             else if(k>=l&&g<m)  a[k][g]=s[k+1][g]; 
             else if(k>=l&&g>=m) a[k][g]=s[k+1][g+1]; 
             else a[k][g]=s[k][g]; 
		} 
         b[z][j]=js(a,n-1); 
	} 
   }
     r=js(s,6);
     for(z=0;z<6;z++)               //求代数余子式，此时b[M][M]中存放的为原矩阵各元素对应的"代数余子式" 
          for(j=0;j<6;j++) 
              if((z+j)%2!=0 && b[z][j]!=0) b[z][j]=-b[z][j]; 
          for(z=0;z<6;z++)        //对b[M][M]转置,此时b[M][M]中存放的为原矩阵的伴随矩阵
           for(j=z+1;j<6;j++) 
		   {
			   temp=b[z][j]; 
               b[z][j]=b[j][z]; 
               b[j][z]=temp; 
		   }
  
     for(z=0;z<6;z++)         //*求逆矩阵，此时b[M][M]中存放的是原矩阵的逆矩阵
          for(j=0;j<6;j++) 
              b[z][j]=b[z][j]/r; 
} 

int main(int argc, char* argv[])
{
	int i,m,j;
	double t,w,k,Xs0,Ys0,Zs0,f,S1=0.0,S2=0.0,m0=0;;
	double x[N],y[N],X[N],Y[N],G[N],Z[N],a[3],b[3],c[3],xo[N],yo[N],l[M][1],A[M][6],B[6][M],C[6][6],D[6][1],E[6][6],T[6][1]={{0},{0},{0},{1},{1},{1}},V[M][1];

   for(i=0;i<N;i++)
	{
		cout<<"请输入第"<<(i+1)<<"个点的影像坐标 x y(mm):"<<endl;
		cin>>x[i]>>y[i];
		cout<<"请输入第"<<(i+1)<<"个点的地面坐标 X Y Z(m)"<<endl;
		cin>>X[i]>>Y[i]>>Z[i];
		S1+=X[i];
		S2+=Y[i];
	}
	

  	for(i=0;i<N;i++)
	{
		x[i]/=1000.0;
	    y[i]/=1000.0;
	}
    t=w=k=0.0;
	m=50000;
	f=0.15324;
	Xs0=S1/N;
	Ys0=S2/N;
    Zs0=m*f;

  while(fabs(T[3][0])>=0.000029||fabs(T[4][0])>=0.000029||fabs(T[5][0])>=0.000029)
	{
    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<N;i++)
	{
		xo[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));
		yo[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<N;i++)
	{
	     A[2*i][0]=(a[0]*f+a[2]*x[i])/G[i];
		 A[2*i][1]=(b[0]*f+b[2]*x[i])/G[i];
		 A[2*i][2]=(c[0]*f+c[2]*x[i])/G[i];
		 A[2*i][3]=y[i]*sin(w)-(x[i]*(x[i]*cos(k)-y[i]*sin(k))/f+f*cos(k))*cos(w);
		 A[2*i][4]=-f*sin(k)-x[i]*(x[i]*sin(k)+y[i]*cos(k))/f;
		 A[2*i][5]=y[i];
		 A[2*i+1][0]=(a[1]*f+a[2]*y[i])/G[i];
		 A[2*i+1][1]=(b[1]*f+b[2]*y[i])/G[i];
		 A[2*i+1][2]=(c[1]*f+c[2]*y[i])/G[i];
		 A[2*i+1][3]=-x[i]*sin(w)-(y[i]*(x[i]*cos(k)-y[i]*sin(k))/f-f*sin(k))*cos(w);
		 A[2*i+1][4]=-f*cos(k)-y[i]*(x[i]*sin(k)+y[i]*cos(k))/f;
		 A[2*i+1][5]=-x[i];

		 l[2*i][0]=x[i]-xo[i];
	     l[2*i+1][0]=y[i]-yo[i];
	}
	
Transpose(A,B);

Matrix_Times1(B,A,C);

Matrix_Times2(B,l,D);

juzhen_ni(C,E,6);

Matrix_Times3(E,D,T);


   Xs0+=T[0][0];
   Ys0+=T[1][0];
   Zs0+=T[2][0];
   t+=T[3][0];
   w+=T[4][0];
   k+=T[5][0];
	}
  
  cout.setf(ios::fixed);
  cout.setf(ios::showpoint);
  cout.precision(2);

   cout<<"像主点的空间坐标为："<<endl;	  
   cout<<"Xs="<<Xs0<<endl;
   cout<<"Ys="<<Ys0<<endl;
   cout<<"Zs="<<Zs0<<endl;

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

  cout.setf(ios::fixed);
  cout.setf(ios::showpoint);
  cout.precision(5);

   cout<<"旋转矩阵R为："<<endl;
   cout<<"a1="<<a[0]<<",a2="<<a[1]<<",a3="<<a[2]<<endl;
   cout<<"b1="<<b[0]<<",b2="<<b[1]<<",b3="<<b[2]<<endl;
   cout<<"c1="<<c[0]<<",c2="<<c[1]<<",c3="<<c[2]<<endl;

  cout.setf(ios::fixed);
  cout.setf(ios::showpoint);
  cout.precision(6);

  cout<<"外方位角元素的限差为："<<endl;
  cout<<"dy="<<T[3][0]<<endl;
    cout<<"dw="<<T[4][0]<<endl;
	  cout<<"dk="<<T[5][0]<<endl;
   
	  for(i=0;i<M;i++)
       for(j=0;j<1;j++)
	   {
	     V[i][j]=0;
	     for(int k=0;k<6;k++)
		 V[i][j]=A[i][k]*T[k][j];
	   }

   for(i=0;i<M;i++)
   {
	   V[i][0]-=l[i][0];
	   m0+=V[i][0]*V[i][0];
	  }
   m0=sqrt(m0/(2*N-6));

   cout<<"单位权中误差为:";
   cout<<m0<<endl;
	return 0;
}