后方交会.cpp

用C++编写的摄影测量后方交汇的程序

#include<iostream.h>
#include<stdio.h>
#include<string.h>
#include <stdlib.h>
#include<malloc.h>
#include<math.h>
#include"matrix.h"
class HouFangJiaoHui
{	 
private:
	 //像点坐标对的个数
	 int Num;	
   	 //像主距
	 double f;
     //外方位直线元素
      double Xs, Ys, Zs;
     //外方位叫元素
      double Fai, K, W;
	 //航高
      double H;
	 //误差阵
	  double X[6][1];	 
	 //系数矩阵A
	  double **A;
	 //旋转矩阵
	  double R[3][3];
	 //坐标对的个数	  
	  double **CoorPicPoint;
	 //地面点坐标
	  double **CoorLandPoint;
	 //微小差值
	  double **L;
	  //协因数矩阵
	  double Q[6][6];
	  //误差限
	  double ErrorLimit;
public:
	//读坐标
	void ReadCoordinates();
	//初始化各个参数
	void Initialize();
	//计算航高，Xs，Ys，Zs
	void HXsYsZs();
	//计算旋转矩阵
	void TryR();
	//计算L
	void TryL();
	//计算系数矩阵A
	void TryA();
	//计算改正数矩阵X
	void TryX();	
	//用改正数改正外方为元素
	void TryResult();   
	//查看是否满足误差限
	bool CheckError();
	//向文件输出
	void OutPut();
	  
};

//读取数据
	 void HouFangJiaoHui::ReadCoordinates()
	 {
		 //文件指针
		FILE *fp; 
		int i,j;
		
		//打开文件
		if((fp=fopen("GivenData.txt","r"))==NULL) 
		{
			cout<<"文件名错误!"<<endl;
			exit(0);
		}
		
		//用于计算数据个数的暂时变量
		float just=0.0;

		//初始化坐标对的个数
	    Num=0;	
		
		//误差限
		fscanf(fp,"%f",&just);	
		ErrorLimit=just;
		//像主距
		fscanf(fp,"%f",&just);	
		f=just/1000;
		 //读取坐标的个数		
		 while(!feof(fp))                    
		 {
			 fscanf(fp,"%f",&just);			
			 Num++;
		 }		
		 
		 //计算坐标对的个数
		 Num/=5;
		 
/*为对应的像点坐标及地面点坐标开辟空间*/
		 //像点坐标
		 CoorPicPoint=(double**)malloc(sizeof(double)*Num);  
		 for(i=0;i<Num;i++)        
			CoorPicPoint[i]=(double*)malloc(sizeof(double)*5);	

		 //地面点坐标			 
		 CoorLandPoint=(double**)malloc(sizeof(double)*Num); 
		 for(i=0;i<Num;i++)      
			CoorLandPoint[i]=(double*)malloc(sizeof(double)*5);
		 
		 //文件指针回到文件的起始位置
		 rewind(fp);
		 
		//存储坐标数组的行标
		 i=0;

		 //越过像主距
		 fscanf(fp,"%f%f",&just,&just);
		 
		 //读取坐标		
		 while(!feof(fp))                     //读取数据的个数
		 {
			 //读像点坐标
			 j=0;
			 fscanf(fp,"%f",&just);
			 CoorPicPoint[i][j++]=just;
			 fscanf(fp,"%f",&just);
			 CoorPicPoint[i][j]=just;

			 //读地面点坐标
			 j=0;
			 fscanf(fp,"%f",&just);
			 CoorLandPoint[i][j++]=just;
			 fscanf(fp,"%f",&just);
			 CoorLandPoint[i][j++]=just;
			 fscanf(fp,"%f",&just);
			 CoorLandPoint[i][j]=just;
			 i++;
		 }
		 fclose(fp);
	
		 //单位换算
		 for (i = 0; i < Num; i++)
			 for (j = 0; j < 2; j++)
				 CoorPicPoint[i][j] /= 1000; 
	 }

	  //初始化
	 void HouFangJiaoHui::Initialize()
	 {			
		 int i,j;
		 //初始化协因数矩阵
		 for(i=0;i<6;i++)
			 for(j=0;j<6;j++)
				 Q[i][j]=0;

		 //初始化系数矩阵A
		 A=(double**)malloc(sizeof(double)*Num*2);  
		 for(i=0;i<Num*2;i++)        
			A[i]=(double*)malloc(sizeof(double)*6);	
		 for(i=0;i<Num*2;i++)
			 for(j=0;j<6;j++)
				 A[i][j]=0;

		 //初始化外方位角元素
		 K=0.0;
		 Fai=0.0;
		 W=0.0;

		 //初始化误差阵
		 for(i=0;i<6;i++)
			X[i][0]=0;

		 //为微小差值矩阵L开辟空间并初始化
		 L=(double**)malloc(sizeof(double)*2*Num);
		 for(i=0;i<2*Num;i++)
			  L[i]=(double*)malloc(sizeof(double));

		 for(i=0;i<2*Num;i++)
			 L[i][0]=0;

		 //旋转矩阵的初始化
		 for(i=0;i<3;i++)			
				 R[i][i]=1;
		 R[1][0]=K;
		 R[2][0]=Fai;
		 R[2][1]=W;
		 R[0][1]=-1*K;
		 R[0][2]=-1*Fai;
		 R[1][2]=W;		 
		 
		 //初始化外方位角元素
		 Fai=0;
		 K=0;
		 W=0;
	 }

	 //求解H,Xs,Ys,Zs
	 void HouFangJiaoHui::HXsYsZs()
	 {
		 //求解地面坐标在X,Y,Z分量上的累加和
		 double x,y,z;
		 x=y=z=0;
		 for (int i = 0; i < Num; i++)
		 {
			 x+= CoorLandPoint[i][0];
			 y+= CoorLandPoint[i][1];
			 z+= CoorLandPoint[i][2];              
		 }
		 Xs = x / Num;
		 Ys = y / Num;
		 Zs = z / Num;		 

		 //求解比例尺，scale中存储的是由各条边求出来的比例尺，最终用RealScale存储比例尺的平均值
		 double *scale;
		 scale=(double*)malloc(sizeof(double)*Num*(Num-1)/2);
		 //FNode代表一条边的起始结点，TNode代表的是一条边的终止结点，ScaleNum代表的是各个比例尺的编号
		 int FNode,TNode,ScaleNum;		 
		 FNode=0;
		 TNode=0;
		 ScaleNum=0;
		 //从第一结点开始计算		 
		 while(FNode<Num)
		 {
			 //为了不重复计算终结点应该从首结点的下一个结点（编号小一的结点）开始
			 for(TNode=FNode+1;TNode<Num;TNode++)
			 {			
				scale[ScaleNum++]=(sqrt(pow(CoorLandPoint[FNode][0]-CoorLandPoint[TNode][0],2)+
						                 pow(CoorLandPoint[FNode][1]-CoorLandPoint[TNode][1],2))/
									sqrt(pow(CoorPicPoint[FNode][0]-CoorPicPoint[TNode][0],2)+
									     pow(CoorPicPoint[FNode][1]-CoorPicPoint[TNode][1],2)));
				
			 }
			  FNode++;
		 }

		//最终的比例尺
		double RealScale,sum=0;
		for(i=0;i<Num*(Num-1)/2;i++)
			sum+=scale[i];
		RealScale=sum/(Num*(Num-1)/2);
		free(scale);	

		 //H值
		 H=RealScale*f;

		 //Zs值
		 Zs+=H;	
	 }
	  
	
	 //求解R值
	 void HouFangJiaoHui::TryR()
	 {
		 for(int i=0;i<3;i++)			
				 R[i][i]=1;
		 //用改正数修改
		 R[0][1]=-1*K;
		 R[0][2]=-1*Fai;
		 R[1][2]=-1*W;
		 R[2][1]=-1*R[1][2];
		 R[1][0]=-1*R[0][1];
		 R[2][0]=-1*R[0][2];
	 }	 

	 //求解L值
	 void HouFangJiaoHui::TryL()
	 {
		 //X0，Y0值，用地面点坐标求解出来的像点坐标
		 double x0Zi,y0Zi,fenMu;
		 double *x0,*y0;		
		 x0=(double*)malloc(sizeof(double)*Num);
		 y0=(double*)malloc(sizeof(double)*Num);		
		 for(int i=0;i<Num;i++)
		 {
			 x0Zi=R[0][0]*(CoorLandPoint[i][0]-Xs)+R[1][0]*(CoorLandPoint[i][1]-Ys)+R[2][0]*(CoorLandPoint[i][2]-Zs);
			 y0Zi=R[0][1]*(CoorLandPoint[i][0]-Xs)+R[1][1]*(CoorLandPoint[i][1]-Ys)+R[2][1]*(CoorLandPoint[i][2]-Zs);
			 fenMu=R[0][2]*(CoorLandPoint[i][0]-Xs)+R[1][2]*(CoorLandPoint[i][1]-Ys)+R[2][2]*(CoorLandPoint[i][2]-Zs);
			 x0[i]=-1*f*(x0Zi/fenMu);
			 y0[i]=-1*f*(y0Zi/fenMu);
		 }

		 //用地面点求解出来的像点坐标与对应的已知的像点坐标之差即为L矩阵
		 int j=0; 
		 
		 for(i=0;i<Num*2;)
		 {			
			 L[i++][0]=CoorPicPoint[j][0]-x0[j];
			 L[i++][0]=CoorPicPoint[j][1]-y0[j++];
		 }	
	 }

	 //求解系数矩阵A；
	 void HouFangJiaoHui::TryA()
	 {
		 int i,j=0;		 

		  for(i=0;i<Num*2;)
		 {
			 A[i][0]=-1*f/H;			
			 A[i][1]=0;		
			 A[i][2]=-1*CoorPicPoint[j][0]/H;	
			 A[i][3]=-1*f*(1+pow(CoorPicPoint[j][0],2)/(f*f));	
			 A[i][4]=-1*CoorPicPoint[j][0]*CoorPicPoint[j][1]/f;	
			 A[i][5]=CoorPicPoint[j][1];	
			 i++;
			 A[i][0]=0;	
			 A[i][1]=-1*f/H;		
			 A[i][2]=-1*CoorPicPoint[j][1]/H;
			 A[i][3]=-1*CoorPicPoint[j][0]*CoorPicPoint[j][1]/f;
			 A[i][4]=-1*f*(1+pow(CoorPicPoint[j][1],2)/(f*f));	
			 A[i][5]=-1*CoorPicPoint[j][0];
			 i++;
			 j++;
		 }	
	 }


	 //计算误差矩阵X
	 void HouFangJiaoHui::TryX()
	 {
		 int i,j,k;
		 //系数矩阵的转置矩阵
		 double **AT;
		 AT=(double**)malloc(sizeof(double)*6);  
		 for(i=0;i<6;i++)        
			AT[i]=(double*)malloc(sizeof(double)*Num*2);

		 for(i=0;i<6;i++)
			 for(j=0;j<Num*2;j++)
				 AT[i][j]=A[j][i];
			
		//AT*A矩阵相乘，结果用Q存储
		double t;
		for(i=0;i<6;i++)
			for(j=0;j<6;j++)
			{ 
				t=0;
				for(k=0;k<Num*2;k++)
					t+=AT[i][k]*A[k][j];
				Q[i][j]=t;
			}		

		//该函数运行之后Q代表的就是协因数矩阵
		int result=inverse(*Q,6);			

	    //Q*AT用temp 存储
		double temp[6][8 ]={0,0};		

		for(i=0;i<6;i++)
			for(j=0;j<Num*2;j++)
			{ 
				t=0;
				for(k=0;k<6;k++)
					t+=Q[i][k]*AT[k][j];
				temp[i][j]=t;
			}

		//X=Q*AT*L即temp*L
		for(i=0;i<6;i++)
			for(j=0;j<1;j++)
			{ 
				t=0;
				for(k=0;k<Num*2;k++)
					t+=temp[i][k]*L[k][j];
				X[i][0]=t;
			}
	 }
	
	 //察看是否满足误差方程
	 bool HouFangJiaoHui::CheckError()
	 {
		 int i;
		 double max;
		 max=X[0][0];
		 for(i=0;i<6;i++)
			 if(X[i][0]>max)
				 max=X[i][0];
		 if(max<0)
			 max*=-1;
		 if(max>ErrorLimit)
			 return false;
		 else
			 return true;
	 }

	 //求解外方位元素
	 void HouFangJiaoHui::TryResult()
	 {
		 Xs+=X[0][0];
		 Ys+=X[1][0];
		 Zs+=X[2][0];
		 Fai+=X[3][0];
		 W+=X[4][0];
		 K+=X[5][0];
	 }

	 //输出结果
	 void HouFangJiaoHui::OutPut()
	 {
		 int i,j;
		 FILE *fp;
		 if((fp=fopen("ResultData.txt","w"))==NULL)  //打开文件
		 {
			 cout<<"Can not open the file!"<<cout;
			 exit(0);
		 }
		 fprintf(fp,"协因数矩阵:\n");		
		 for(i=0;i<6;i++)
		 {
			 for(j=0;j<6;j++)
				 if(Q[i][j]<0)
					 fprintf(fp,"%10.9f     ",Q[i][j]); 
				 else
					 fprintf(fp,"%10.9f    ",Q[i][j]); 
			  fprintf(fp,"\n");
		 }
		 fprintf(fp,"计算外方位元素的结果:\n");  
		 fprintf(fp,"%s%f%s%10.9f%s%10.9f%s%10.9f%s%10.9f%s%10.9f","Xs:",Xs,"   Ys:",Ys,"   Zs:",Zs,"   Fai:",Fai,"   K:",K,"   W:",W);
		 fclose(fp);
	 }

//程序主函数
void main()
{
	//构造一个对象
	HouFangJiaoHui houFang;
	//读取给定坐标及像主距，误差限
	houFang.ReadCoordinates();
	//初始化各个参数
	houFang.Initialize();
	//计算航高
	houFang.HXsYsZs();	
	do
	{
		//计算旋转矩阵R
		houFang.TryR();	
		houFang.TryL();	
		//计算系数矩阵A
		houFang.TryA();	
		//计算改正数
		houFang.TryX();	
		//用改正数改正外方为元素
		houFang.TryResult();	
	}
	//查看是否满足误差限的要求
	while(houFang.CheckError()==false);	
	//输出结果
	houFang.OutPut();
	cout<<"结果已经输出到GivenData.txt中"<<endl;
	getchar();
}