backinsection.cpp

摄影测量利用空间后方交汇的方法编写的可以从相片像平面坐标获得该点实际的物方坐标,得到空间定位的数据

// BackInsection.cpp: implementation of the CBackInsection class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "单像空间后方交会.h"
#include "BackInsection.h"
#include "math.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////
//构造函数
CBackInsection::CBackInsection(double x0,double y0,double f,double x[],double y[],double XX[],double YY[],double ZZ[],int N)
{
   fx0=x0;
   fy0=y0;
   ff=f;
   iN=N;

   fZs=2000;
   fXs=fYs=0;

   for(int k=0;k<iN;k++)
   {
     fXs+=XX[k];
	 fYs+=YY[k];
   }
   fXs/=iN;fYs/=iN;//初始值用平均值

   fphi=fomica=fkapa=0;

   fpx=new double[iN];
   fpy=new double[iN];
   fp_X=new double[iN];
   fp_Y=new double[iN];
   fp_Z=new double[iN];
  
   if(fpx && fpy && fp_X && fp_Y && fp_Z)	  
	   for(int i=0;i<iN;i++)
	   {
		   fpx[i]=x[i];
		   fpy[i]=y[i];
		   fp_X[i]=XX[i];
		   fp_Y[i]=YY[i];
		   fp_Z[i]=ZZ[i];
	   } 
   else
	   AfxMessageBox("初始化失败！");

   fpApx_x=new double[iN];
   fpApx_y=new double[iN];
   if(!fpApx_x)
	   AfxMessageBox("初始化失败！");
   if(!fpApx_y)
	   AfxMessageBox("初始化失败！");

}
////////////////////////////////////////////////////////////////////////////
//析构函数
CBackInsection::~CBackInsection()
{
  
	if(fpx)
		delete[]fpx;

    if(fpy)
		delete[]fpy;

	if(fp_X)
		delete[]fp_X;

	if(fp_Y)
		delete[]fp_Y;

	if(fp_Z)
		delete[]fp_Z;

	if(fpApx_x)
		delete[]fpApx_x;

	if(fpApx_y)
		delete[]fpApx_y;

   
}
///////////////////////////////////////////////////////////////////////////
//计算旋转矩阵
CMatrix CBackInsection::RotateMatrix(double phi,double omica,double kapa)
{
	
	std::vector<double>rotate(9);
	rotate[0]=cos(phi)*cos(kapa)-sin(phi)*sin(omica)*sin(kapa);
	rotate[1]=-cos(phi)*sin(kapa)-sin(phi)*sin(omica)*cos(kapa);
	rotate[2]=-sin(phi)*cos(omica);
	rotate[3]=cos(omica)*sin(kapa);
	rotate[4]=cos(omica)*cos(kapa);
	rotate[5]=-sin(omica);
	rotate[6]=sin(phi)*cos(kapa)+cos(phi)*sin(omica)*sin(kapa);
	rotate[7]=-sin(phi)*sin(kapa)+cos(phi)*sin(omica)*cos(kapa);
	rotate[8]=cos(phi)*cos(omica);
  
	 CMatrix mat(rotate,3,3);
	 return mat;
} 
//////////////////////////////////////////////////////////////////////////////
//求误差方程式的系数矩阵
CMatrix CBackInsection::CoefficientMatrix(double x,double y,double avgZ,CMatrix& matR)
{	

	std::vector<double>Coefficient(12);
    Coefficient[0]=1/avgZ*(matR[0]*ff+matR[2]*(x-fx0));
	Coefficient[1]=1/avgZ*(matR[3]*ff+matR[5]*(x-fx0));
	Coefficient[2]=1/avgZ*(matR[6]*ff+matR[8]*(x-fx0));
	Coefficient[6]=1/avgZ*(matR[1]*ff+matR[2]*(y-fy0));
	Coefficient[7]=1/avgZ*(matR[4]*ff+matR[5]*(y-fy0));
	Coefficient[8]=1/avgZ*(matR[7]*ff+matR[8]*(y-fy0));
	Coefficient[3]=(y-fy0)*sin(fomica)-((x-fx0)/ff*((x-fx0)*cos(fkapa)-(y-fy0)*sin(fkapa))+ff*cos(fkapa))*cos(fomica);
	Coefficient[4]=-ff*sin(fkapa)-(x-fx0)/ff*((x-fx0)*sin(fkapa)+(y-fy0)*cos(fkapa));
	Coefficient[5]=y-fy0;
	Coefficient[9]=-(x-fx0)*sin(fomica)-((y-fy0)/ff*((x-fx0)*cos(fkapa)-(y-fy0)*sin(fkapa))-ff*sin(fkapa))*cos(fomica);//
	Coefficient[10]=-ff*cos(fkapa)-(y-fy0)/ff*((x-fx0)*sin(fkapa)+(y-fy0)*cos(fkapa));
	Coefficient[11]=-(x-fx0);

	CMatrix mat(Coefficient,2,6);
	return mat;

} 

//////////////////////////////////////////////////////////////////////////////
//求误差方程式的常数矩阵
CMatrix CBackInsection::ConstantMatrix(double x,double y,double apx_x,double apx_y)
{
	std::vector<double>Constant(2);
	Constant[0]=x-apx_x;
	Constant[1]=y-apx_y;

	CMatrix mat(Constant,2,1);
	return mat;
}
///////////////////////////////////////////////////////////////////////////////
//主处理函数
void CBackInsection::Process(void)
{   
 
	std::vector<double>A,L; 
	CMatrix temA,temL;
	CString S;
	double avgZ=0.0;
	int nLoop=0;//循环次数

  while(1)
   {
	   A.clear();
	   L.clear();   
	   
       //计算旋转矩阵R
	   R=RotateMatrix(fphi,fomica,fkapa);  
       //if(nLoop==2)
		  // R.Display(); 
	   for(int i=0;i<iN;i++)
	   {   
		  //计算(x),(y)
		   fpApx_x[i]=fx0-ff*(R[0]*(fp_X[i]-fXs)+R[3]*(fp_Y[i]-fYs)+R[6]*(fp_Z[i]-fZs))
			   /(R[2]*(fp_X[i]-fXs)+R[5]*(fp_Y[i]-fYs)+R[8]*(fp_Z[i]-fZs));
		   
		   fpApx_y[i]=fy0-ff*(R[1]*(fp_X[i]-fXs)+R[4]*(fp_Y[i]-fYs)+R[7]*(fp_Z[i]-fZs))
			   /(R[2]*(fp_X[i]-fXs)+R[5]*(fp_Y[i]-fYs)+R[8]*(fp_Z[i]-fZs));	 
           
		   avgZ=R[2]*(fp_X[i]-fXs)+R[5]*(fp_Y[i]-fYs)+R[8]*(fp_Z[i]-fZs);
		   
		   //if(nLoop==1)
		   //{
			   //S.Format("%f,%f",fpApx_x[i],fpApx_y[i]); 
			   //AfxMessageBox(S);
		  //}

		   //计算误差方程式的系数矩阵和常数项矩阵
		   temA=CoefficientMatrix(fpx[i],fpy[i],avgZ,R);
		   temL=ConstantMatrix(fpx[i],fpy[i],fpApx_x[i],fpApx_y[i]);
		   
		   for(int n=0;n<temA.GetSize();n++)			   
			  A.push_back(temA[n]);			   
	
		   for(int m=0;m<temL.GetSize();m++)		
			  L.push_back(temL[m]);		  
		  
	   }	
	   matA=CMatrix(A,2*iN,6);      
	   
	   //if(nLoop==1)
	      //matA.Display(); 
	   matL=CMatrix(L,2*iN,1);	
	   //if(nLoop==1)
	     // matL.Display(); 
	   matX=~(matA.T()*matA)*matA.T()*matL;	//解法方程 
	   
	  //if(nLoop==1)
          //matX.Display(); 

	   fXs+=matX[0];
	   fYs+=matX[1];
	   fZs+=matX[2];
	   fphi+=matX[3];
	   fomica+=matX[4];
	   fkapa+=matX[5];
      
	   //if(nLoop==1)
	   //{
		   //S.Format("%f,%f,%f,%f,%f,%f",fXs,fYs,fZs,fphi,fomica,fkapa); 
		   //AfxMessageBox(S);
	   //}
	   //if(nLoop==1)
		   //matX.Display();
	  

	   if(fabs(matX[3]/PI*180*60)<0.1 && fabs(matX[4]/PI*180*160)<0.1 && fabs(matX[5]/PI*180*60)<0.1)
	   {
		   //S.Format("%d次",++nLoop); 
		   //AfxMessageBox("成功收敛于第"+S);		   
           return;
	   }
       

	   if(++nLoop>=20)
	   {
		   AfxMessageBox("不能收敛!!");
		   return;
	   }
		  
   }
	
} 

////////////////////////////////////////////////////////////////
//显示
void CBackInsection::Display(void)
{
	CString S;
	S.Format("%f,%f,%f",fXs,fYs,fZs);
	AfxMessageBox(S);
	/*
	for(int i=0;i<iN;i++)
	{
		S.Format("%f,%f,%f,%f,%f",fpx[i],fpy[i],fp_X[i],fp_Y[i],fp_Z[i]);
		AfxMessageBox(S);
	}
	*/
}