rearmeet.cpp

摄影测量中进行后方交会的程序

//RearMeet.cpp: implementation of the CRearMeet class.

////////////////////////////////////////////////////////////////////
#include "stdafx.h"
#include "RearMeet.h"
#include "math.h"

#define PI 3.14159265358979
////////////////////////////////////////////////////////////////////
//Construction/Destruction
////////////////////////////////////////////////////////////////////
CRearMeet::CRearMeet()
{
}

CRearMeet::~CRearMeet()
{
}

/*
  功能：构造函数
  参数：df：主距； dm：摄影比例尺分母； in：控制点个数； x0 y0：像主点在框标坐标系的坐标；
  maPointXYZ：相片地面点坐标值； dk：航片旋转角； dw：旁向偏角； da：航向偏角
  返回值：
  说明：
*/
CRearMeet::CRearMeet(double df, double dm, int in,  double dx0, double dy0, CMatrix maPointxyXYZ)
{ 
 	m_pDataFIlE = fopen("Data.txt", "w");
	m_iCount = 0;
	m_df = df;
	m_dm = dm;
	m_iPointNum = in;
	m_dx0 = dx0;
	m_dy0 = dy0;
	m_maPointxyXYZ = maPointxyXYZ;
	m_maR.Initate(3,3);
	m_maB.Initate(2*m_iPointNum,6);
	m_maL.Initate(2*m_iPointNum,1);
	m_maX0.Initate(6,1);
	m_madX.Initate(6,1);
	m_maX.Initate(6,1);
	m_maV.Initate(2*m_iPointNum,1);
    Initate();
	UpdateRMatrix();
	UpdateBMatrix();
	UpdateLMatrix();
	
}
/*
  功能：初始化函数
  参数：df：主距； dm：摄影比例尺分母； in：控制点个数； x0 y0：像主点在框标坐标系的坐标；
  maPointXYZ：相片地面点坐标值； dk：航片旋转角； dw：旁向偏角； da：航向偏角
  返回值：成功返回 1 
  说明：
*/
int CRearMeet::Initate()
{
	m_da = 0;
	m_dw = 0;
	m_dk = 0;
	
	m_dXSYSZS[2] = m_df * m_dm;
	m_dXSYSZS[0] = m_dXSYSZS[1] = 0.0;
	double* pdPoint = m_maPointxyXYZ.GetData();
	int iLine = m_maPointxyXYZ.GetLine();
	for(int i=0; i<m_iPointNum; i++)
	{
		m_dXSYSZS[0] += pdPoint[i*iLine+2];
		m_dXSYSZS[1] += pdPoint[i*iLine+3];
	}
	m_dXSYSZS[0] /= m_iPointNum;
	m_dXSYSZS[1] /= m_iPointNum;

	double* pdX0 = m_maX0.GetData();
	pdX0[0] = m_dXSYSZS[0];
	pdX0[1] = m_dXSYSZS[1];
	pdX0[2] = m_dXSYSZS[2];
	pdX0[3] = m_da;
	pdX0[4] = m_dw;
	pdX0[5] = m_dk;
	return 1;
}
/*
  功能：主要的计算函数
  参数：无
  返回值：计算成功返回 1， 失败返回0 
  说明：
*/
int CRearMeet::Calculate()
{
	m_iCount = 0;
	int iResult = 0;
	do 
	{
		m_iCount++;
//		double* pdmaV = m_maV.GetData();
//		double* pdmaPoint = m_maPointxyXYZ.GetData();
//		int imaPointLine = m_maPointxyXYZ.GetLine();
//		for(int i=0; i<m_iPointNum; i++)
//		{
//			pdmaPoint[i*imaPointLine] += pdmaV[2*i];
//			pdmaPoint[i*imaPointLine+1] += pdmaV[2*i+1];
//   		}
		
		if(m_iCount != 1)
		{
			double* pdX0 = m_maX0.GetData();
			m_dXSYSZS[0] = pdX0[0];
			m_dXSYSZS[1] = pdX0[1];
			m_dXSYSZS[2] = pdX0[2];
			m_da = pdX0[3];
			m_dw = pdX0[4];
			m_dk = pdX0[5];
			UpdateRMatrix();
			UpdateBMatrix();
			UpdateLMatrix();
		}
		CMatrix maBT = m_maB;
		fprintf(m_pDataFIlE, "第%d次迭代\n", m_iCount);
		fprintf(m_pDataFIlE, "-----------m_maB------------\n");
		FileOperate(m_maB);
		fprintf(m_pDataFIlE, "-----------m_maL------------\n");
		FileOperate(m_maL);
		maBT.transpose();
		CMatrix maBTB = maBT*m_maB;
		CMatrix maConveBTB = maBTB.GetConvertMatrix();
		m_maDx = maConveBTB;
		fprintf(m_pDataFIlE, "-----------m_maDx------------\n");

		FileOperate(m_maDx);
		m_madX = maConveBTB*maBT*m_maL;
 		m_maV = m_maB*m_madX - m_maL;
		m_maX0  = m_maX0 + m_madX;
		iResult = Judge();
		fprintf(m_pDataFIlE, "-----------m_maV------------\n");

		FileOperate(m_maV);
	} 
	while(iResult == -1&& m_iCount <= 5);
	if(m_iCount == 6) //计算不成功
		return 0;
	printf("%d\n", m_iCount);
	
	m_maV = m_maB*m_madX - m_maL;
	CMatrix maVT = m_maV;
	maVT.transpose();
	double* pdSigma;
	CMatrix maVTPV = maVT*m_maV;
	pdSigma = (maVTPV).GetData();
	m_dSigma = sqrt(pdSigma[0]/(2*m_iPointNum-6));
	fprintf(m_pDataFIlE, "---------------Sigma-----------\n");
	fprintf(m_pDataFIlE, "%.18lf", m_dSigma);
	m_maX = m_maX0;
	double* pdX = m_maX.GetData();
	m_dXSYSZS[0] = pdX[0];
	m_dXSYSZS[1] = pdX[1];
	m_dXSYSZS[2] = pdX[2];
	m_da = pdX[3];
	m_dw = pdX[4];
	m_dk = pdX[5];
	UpdateRMatrix();
	double* pdmaDx = m_maDx.GetData();
	printf("%.8lf, %.8lf, %.8lf, %.8lf, %.8lf, %.8lf\n", 
		sqrt(pdmaDx[0])*m_dSigma,sqrt(pdmaDx[7])*m_dSigma,sqrt(pdmaDx[14])*m_dSigma,
		sqrt(pdmaDx[21])*m_dSigma,sqrt(pdmaDx[28])*m_dSigma,sqrt(pdmaDx[35])*m_dSigma);

 	m_maR.DisplayMatrix();
// 	printf("%d\n",iCount);
	ResultOutPut();
	return 1;
}

int CRearMeet::UpdateBMatrix()
{
	double* pdB = m_maB.GetData();
	int ipdBLine = m_maB.GetLine();
	double* pdR = m_maR.GetData();
	int ipdRLine = m_maR.GetLine();
	double* pdXYZ = m_maPointxyXYZ.GetData();
	int ipdXYZLine = m_maPointxyXYZ.GetLine();

	double a_cos = cos(m_da);
	double a_sin = sin(m_da);
	double w_cos = cos(m_dw);
	double w_sin = sin(m_dw);
	double k_cos = cos(m_dk);
	double k_sin = sin(m_dk);
	double dZ = 0.0;
	double dx = 0.0;
	double dy = 0.0;
	for(int i=0; i<m_iPointNum; i++)
	{	
		
//		dx = pdXYZ[i*ipdXYZLine+0];
//    	dy = pdXYZ[i*ipdXYZLine+1];
		dZ = pdR[0*ipdRLine+2]*(pdXYZ[i*ipdXYZLine+2]-m_dXSYSZS[0]) + 
			 pdR[1*ipdRLine+2]*(pdXYZ[i*ipdXYZLine+3]-m_dXSYSZS[1]) +
			 pdR[2*ipdRLine+2]*(pdXYZ[i*ipdXYZLine+4]-m_dXSYSZS[2]);
		
		dx = pdR[0*ipdRLine+0]*(pdXYZ[i*ipdXYZLine+2]-m_dXSYSZS[0]) + 
			 pdR[1*ipdRLine+0]*(pdXYZ[i*ipdXYZLine+3]-m_dXSYSZS[1]) +
			 pdR[2*ipdRLine+0]*(pdXYZ[i*ipdXYZLine+4]-m_dXSYSZS[2]);
		
		dy = pdR[0*ipdRLine+1]*(pdXYZ[i*ipdXYZLine+2]-m_dXSYSZS[0]) + 
			 pdR[1*ipdRLine+1]*(pdXYZ[i*ipdXYZLine+3]-m_dXSYSZS[1]) +
			 pdR[2*ipdRLine+1]*(pdXYZ[i*ipdXYZLine+4]-m_dXSYSZS[2]);	
		dx = m_dx0 - m_df*dx/dZ;
     		dy = m_dy0 - m_df*dy/dZ;

		int i1 = i*2;
		
		pdB[i1*ipdBLine+0] = (pdR[0*ipdRLine+0]*m_df + pdR[0*ipdRLine+2]*(dx-m_dx0))/dZ;
		pdB[i1*ipdBLine+1] = (pdR[1*ipdRLine+0]*m_df + pdR[1*ipdRLine+2]*(dx-m_dx0))/dZ;
		pdB[i1*ipdBLine+2] = (pdR[2*ipdRLine+0]*m_df + pdR[2*ipdRLine+2]*(dx-m_dx0))/dZ;
		pdB[i1*ipdBLine+3] = (dy-m_dy0)*w_sin - 
					         w_cos*( m_df*k_cos+(dx-m_dx0)*((dx-m_dx0)
					         *k_cos-(dy-m_dy0)*k_sin)/m_df );
		pdB[i1*ipdBLine+4] = -m_df*k_sin - 
					         (dx-m_dx0)*((dx-m_dx0)*k_sin+(dy-m_dy0)*k_cos)/m_df;
		pdB[i1*ipdBLine+5] = dy - m_dy0;
		
		int i2 = i1 + 1;
		pdB[i2*ipdBLine+0] = (pdR[0*ipdRLine+1]*m_df + pdR[0*ipdRLine+2]*(dy-m_dy0))/dZ;
		pdB[i2*ipdBLine+1] = (pdR[1*ipdRLine+1]*m_df + pdR[1*ipdRLine+2]*(dy-m_dy0))/dZ;
		pdB[i2*ipdBLine+2] = (pdR[2*ipdRLine+1]*m_df + pdR[2*ipdRLine+2]*(dy-m_dy0))/dZ;
		pdB[i2*ipdBLine+3] = -(dx-m_dx0)*w_sin - 
					         w_cos*( -m_df*k_sin+(dy-m_dy0)*((dx-m_dx0)
					         *k_cos-(dy-m_dy0)*k_sin)/m_df );
		pdB[i2*ipdBLine+4] = -m_df*k_cos - 
					         (dy-m_dy0)*((dx-m_dx0)*k_sin+(dy-m_dy0)*k_cos)/m_df;
		pdB[i2*ipdBLine+5] = m_dx0 - dx;		
	}
	return 1;
}

int CRearMeet::UpdateLMatrix()
{
	double* pdL = m_maL.GetData();
	double* pdxyXYZ = m_maPointxyXYZ.GetData();
	int ipdXYZLine = m_maPointxyXYZ.GetLine();
	double dX = 0.0;
	double dY = 0.0;
	double dZ = 0.0;
	double dXYZXYZS[3] = {0.0};
	CMatrix maXYZXYZS;
	for(int i=0; i<m_iPointNum; i++)
	{
		dXYZXYZS[0] = pdxyXYZ[i*ipdXYZLine+2] - m_dXSYSZS[0];
		dXYZXYZS[1] = pdxyXYZ[i*ipdXYZLine+3] - m_dXSYSZS[1];
		dXYZXYZS[2] = pdxyXYZ[i*ipdXYZLine+4] - m_dXSYSZS[2];
		maXYZXYZS.CreateData(dXYZXYZS, 3, 1);
		CMatrix maRTra = m_maR;
		maRTra.transpose();
		CMatrix maXYZ = maRTra*maXYZXYZS;
		double* pdXYZ = maXYZ.GetData();
		int i1 = i*2;
		pdL[i1] = pdxyXYZ[i*ipdXYZLine+0] - (-m_df*pdXYZ[0]/pdXYZ[2] + m_dx0)  ;
		pdL[i1+1] = pdxyXYZ[i*ipdXYZLine+1] - (-m_df*pdXYZ[1]/pdXYZ[2] + m_dy0) ;
	}
	return 1;
}

int CRearMeet::UpdateRMatrix()
{
	double* pdR = m_maR.GetData();
	int iLine = m_maR.GetLine();
	double a_cos = cos(m_da);
	double a_sin = sin(m_da);
	double w_cos = cos(m_dw);
	double w_sin = sin(m_dw);
	double k_cos = cos(m_dk);
	double k_sin = sin(m_dk);
	pdR[0] = a_cos*k_cos - a_sin*w_sin*k_sin;
	pdR[1] = - a_cos*k_sin - a_sin*w_sin*k_cos;
	pdR[2] = - a_sin * w_cos;
	pdR[3] = w_cos*k_sin;
	pdR[4] = w_cos*k_cos;
	pdR[5] = - w_sin;
	pdR[6] = a_sin*k_cos + a_cos*w_sin*k_sin;
	pdR[7] = - a_sin*k_sin + a_cos*w_sin*k_cos;
	pdR[8] = a_cos * w_cos;
	return 1;
}

int CRearMeet::Judge()
{
	double* pdDx = m_madX.GetData();
	int iLine = m_madX.GetLine();
	double dTriError = 3e-5;
	if(fabs(pdDx[0])<=0.01 && fabs(pdDx[1])<=0.01 && fabs(pdDx[2])<=0.01 &&
	   fabs(pdDx[3])<=dTriError && fabs(pdDx[4])<=dTriError && fabs(pdDx[5])<=dTriError)
		return 1;
	else 
		return -1;
}

void CRearMeet::Display()
{
	double* pdX = m_maX.GetData();
	for(int i=0; i<6; i++)
		printf("%10.5lf  ",pdX[i]);
	printf("\n");
}

void CRearMeet::FileOperate(CMatrix maOutput)
{
	int iRow = maOutput.GetRow();
	int iLine = maOutput.GetLine();
	double* pdma = maOutput.GetData();
	for(int i=0; i<iRow; i++)
	{
		for(int j=0; j<iLine; j++)
			fprintf(m_pDataFIlE, "%20.7lf ", pdma[i*iLine+j]);
		fprintf(m_pDataFIlE, "\n");	
	}
	fprintf(m_pDataFIlE, "\n");
 }

void CRearMeet::ResultOutPut()
{
	fprintf(m_pDataFIlE, "-----------------------------------------------------\n");
	fprintf(m_pDataFIlE, "                     空间后方交会计算\n");
	fprintf(m_pDataFIlE, "-----------------------------------------------------\n");
	fprintf(m_pDataFIlE, "             影像坐标             地面坐标\n");
	fprintf(m_pDataFIlE, "-----------------------------------------------------\n");
	fprintf(m_pDataFIlE, "-------x(mm)-----y(mm)---|----X(m)------Y(m)------Z(m)-------\n");
	double* pfmaxyXYZ = m_maPointxyXYZ.GetData();
	for(int i=0; i<4; i++)
	{
	fprintf(m_pDataFIlE, "    %8.2lf  %8.2lf   |  %8.2lf  %8.2lf %8.2lf\n",
		pfmaxyXYZ[i*5]*1000,pfmaxyXYZ[i*5+1]*1000,pfmaxyXYZ[i*5+2],pfmaxyXYZ[i*5+3],pfmaxyXYZ[i*5+4]);
	}
}