后方交会.cpp

编写一个完整的单片空间后方交会程序

#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])   //将矩阵a[]转置，存储在b[]中
{
	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];//c[][]存放矩阵的乘积
 }
 
}
//计算逆矩阵
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;
 int  nCount=0;
double n=0.0;
 double t,w,k,Xs0,Ys0,Zs0,f,S1=0.0,S2=0.0;
 double jd[6];
 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];
cout<<"请输入摄影比例尺："<<endl;

cin>>m;
cout<<endl;
cout<<"请输入主焦距："<<endl;
cin>>f;
cout<<endl;
   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.000001||fabs(T[4][0])>=0.000001||fabs(T[5][0])>=0.000001) //控制输出精度
 {
 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);//y[i]的分子

 }

    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];
   nCount++;

 }
//函数调用
 
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(3);

   cout<<"像主点的空间坐标为："<<endl;   
   cout<<"Xs="<<Xs0<<endl;
   cout<<"Ys="<<Ys0<<endl;
   cout<<"Zs="<<Zs0<<endl;
   cout<<"角元素为："<<endl;
   cout<<"φ="<<t<<endl;
   cout<<"ω="<<w<<endl;  
   cout<<"κ="<<k<<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(10);

  cout<<"外方位角元素的限差为："<<endl;
  cout<<"dy="<<T[3][0]<<endl;
    cout<<"dw="<<T[4][0]<<endl;
   cout<<"dk="<<T[5][0]<<endl;
 

   for(i=0;i<N;i++)
   {
    V[2*i][0]=A[2*i][0]*T[0][0]+A[2*i][1]*T[1][0]+A[2*i][2]*T[2][0]+A[2*i][3]*T[3][0]+A[2*i][4]*T[4][0]+A[2*i][5]*T[5][0]-l[2*i][0];
       V[2*i+1][0]=A[2*i+1][0]*T[0][0]+A[2*i+1][1]*T[1][0]+A[2*i+1][2]*T[2][0]+A[2*i+1][3]*T[3][0]+A[2*i+1][4]*T[4][0]+A[2*i+1][5]*T[5][0]-l[2*i+1][0];
      n+=V[2*i][0]*V[2*i][0]+V[2*i+1][0]*V[2*i+1][0];
	     //nCount++;
   }   
   n=sqrt(n/(2*N-6));

   cout<<"单位权中误差m0为:";
   cout<<n<<endl;
   cout<<"Xs,Ys,ZS,φ,ω,κ的精度分别为："<<endl;
   for(i=0;i<6;i++)
   {
	   jd[i]=(sqrt(E[i][i]))*n;
	   
       cout<<"第"<<i+1<<"个外方位元素的精度为："<<jd[i]<<endl;
   }
   
   cout<< "迭代次数为:"<<nCount<<endl;
 return 0;
}