readdlg.cpp

..三维DLT变换解决外方位元素的求解以及对11个参数的确定

// ReadDlg.cpp : implementation file
//

#include "stdafx.h"
#include "DLT.h"
#include "ReadDlg.h"
#include "fstream.h"
#include "Matrix.h"
#include "math.h"

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

/////////////////////////////////////////////////////////////////////////////
// CReadDlg dialog


CReadDlg::CReadDlg(CWnd* pParent /*=NULL*/)
	: CDialog(CReadDlg::IDD, pParent)
{
	//{{AFX_DATA_INIT(CReadDlg)
	m_XD = _T("");
	m_KZD = _T("");
	//}}AFX_DATA_INIT
}


void CReadDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
	//{{AFX_DATA_MAP(CReadDlg)
	DDX_Text(pDX, IDC_EDIT1, m_XD);
	DDX_Text(pDX, IDC_EDIT2, m_KZD);
	//}}AFX_DATA_MAP
}


BEGIN_MESSAGE_MAP(CReadDlg, CDialog)
	//{{AFX_MSG_MAP(CReadDlg)
	ON_BN_CLICKED(IDC_BUTTON1, OnButton1)
	ON_BN_CLICKED(IDC_BUTTON2, OnButton2)
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CReadDlg message handlers

void CReadDlg::OnButton1() 
{
	// TODO: Add your control notification handler code here
	CString strOpenFilter = "txt文件 (*.txt)|*.txt||";
	CFileDialog FileDlg(TRUE, "*.txt", NULL, OFN_HIDEREADONLY | OFN_OVERWRITEPROMPT, strOpenFilter);
	if(FileDlg.DoModal()==IDOK)
	{
		m_XD=FileDlg.GetPathName();
	}
	UpdateData(FALSE);	
}

void CReadDlg::OnButton2() 
{
	// TODO: Add your control notification handler code here
	CString strOpenFilter = "txt文件 (*.txt)|*.txt||";
	CFileDialog FileDlg(TRUE, "*.txt", NULL, OFN_HIDEREADONLY | OFN_OVERWRITEPROMPT, strOpenFilter);
	if(FileDlg.DoModal()==IDOK)
	{
		m_KZD=FileDlg.GetPathName();
	}
	UpdateData(FALSE);	
}

void CReadDlg::OnOK() 
{
	// TODO: Add extra validation here
	CString Idata=m_XD;
		if(Idata=="")
		{
			return;
		}
	CString Jdata=m_KZD;
		if(Jdata=="")
		{
			return;
		}
    double DH1[23],DH2[199],XDx[23],XDy[23],KZDx[199],KZDy[199],KZDz[199];
    //double XD[23][2],KZD[199][3];
    int i;
	ifstream in;
	in.open(Idata);
	for (i=0;i<23;i++)
	{
		in>>DH1[i]>>XDx[i]>>XDy[i];
	}
	in.close();
    
    in.open(Jdata);
	for (i=0;i<199;i++)
	{
		in>>DH2[i]>>KZDx[i]>>KZDy[i]>>KZDz[i];
	}
	in.close();

	double ZHXDx[23],ZHXDy[23];
	for(i=0;i<23;i++)
	{
		ZHXDx[i]=0.0055*(XDx[i]-2144);
		ZHXDy[i]=0.0055*(1424-XDy[i]);
	}
    
	int p;
    double PPKZDx[23],PPKZDy[23],PPKZDz[23];
	for(p=0;p<23;p++)
	{
		for(i=0;i<23;i++)
		{   
		int j;
		for(j=0;j<199;j++)
		{
			if(DH1[i]==DH2[j])
			   PPKZDx[p]=KZDx[j];
			   PPKZDy[p]=KZDy[j]; 
               PPKZDz[p]=KZDz[j];  
		}
		}
	}

    double A[23*11*2],L[23*2];
	for (i=0;i<23;i++)
	{
		A[22*i+0]=PPKZDx[i];
        A[22*i+1]=PPKZDy[i];
		A[22*i+2]=PPKZDz[i];
		A[22*i+3]=1;
		A[22*i+4]=0;
	    A[22*i+5]=0;
        A[22*i+6]=0;
    	A[22*i+7]=0;
		A[22*i+8]=ZHXDx[i]*PPKZDx[i];
        A[22*i+9]=ZHXDx[i]*PPKZDy[i];
		A[22*i+10]=ZHXDx[i]*PPKZDz[i];
		A[22*i+11]=0;
	    A[22*i+12]=0;
        A[22*i+13]=0;
    	A[22*i+14]=0;
        A[22*i+15]=PPKZDx[i];
        A[22*i+16]=PPKZDy[i];
		A[22*i+17]=PPKZDz[i];
        A[22*i+18]=1;
        A[22*i+19]=ZHXDy[i]*PPKZDx[i];
        A[22*i+20]=ZHXDy[i]*PPKZDy[i];
		A[22*i+21]=ZHXDy[i]*PPKZDz[i];
	}

	for (i=0;i<23;i++)
	{
		L[2*i+0]=-ZHXDx[i];
        L[2*i+1]=-ZHXDy[i];
	}
	
    CMatrix AA,AT,ATA,invATA;
	AA.InitMatrix(A,46,11);
	AA.GetTransposedMatrix(AT);
	ATA=AT*AA;
	ATA.GetInverseMatrix(invATA); 
	
    CMatrix CL;
	CL.InitMatrix(L,46,1);

	CMatrix X;
	X=invATA*AT*CL;

	double x0,y0;
	double b1,b2,a3,b3,c3,a2,R,p1,p2,p3,dB,ds,fx,fy;
	double tanF,sinO,tanK;

	  R=1.0/(X.m_pData[8]*X.m_pData[8]+X.m_pData[9]*X.m_pData[9]+X.m_pData[10]*X.m_pData[10]);
	  x0=-(X.m_pData[0]*X.m_pData[8]+X.m_pData[1]*X.m_pData[9]+X.m_pData[2]*X.m_pData[10])/(X.m_pData[8]*X.m_pData[8]+X.m_pData[9]*X.m_pData[9]+X.m_pData[10]*X.m_pData[10]);
	  y0=-(X.m_pData[4]*X.m_pData[8]+X.m_pData[5]*X.m_pData[9]+X.m_pData[6]*X.m_pData[10])/(X.m_pData[8]*X.m_pData[8]+X.m_pData[9]*X.m_pData[9]+X.m_pData[10]*X.m_pData[10]);
	  p1=R*(X.m_pData[0]*X.m_pData[0]+X.m_pData[1]*X.m_pData[1]+X.m_pData[2]*X.m_pData[2])-x0*x0;
	  p2=R*(X.m_pData[4]*X.m_pData[4]+X.m_pData[5]*X.m_pData[5]+X.m_pData[6]*X.m_pData[6])-y0*y0;
	  p3=R*(X.m_pData[0]*X.m_pData[4]+X.m_pData[1]*X.m_pData[5]+X.m_pData[2]*X.m_pData[6])-x0*y0;
	  dB=asin(sqrt(p3*p3*1.0/p1/p2));
	  ds=sqrt(p1*1.0/p2)-1;
	  fx=sqrt(p1)*cos(p2);
	  fy=fx/(1+ds);

	  a3=X.m_pData[8]/sqrt(X.m_pData[8]*X.m_pData[8]+X.m_pData[9]*X.m_pData[9]+X.m_pData[10]*X.m_pData[10]);
	  b3=X.m_pData[9]/sqrt(X.m_pData[8]*X.m_pData[8]+X.m_pData[9]*X.m_pData[9]+X.m_pData[10]*X.m_pData[10]);
	  c3=X.m_pData[10]/sqrt(X.m_pData[8]*X.m_pData[8]+X.m_pData[9]*X.m_pData[9]+X.m_pData[10]*X.m_pData[10]);
	  a2=sqrt(R)*(X.m_pData[4]+X.m_pData[8]*y0)*(1+ds)*cos(dB)/fx;
	  b2=(sqrt(R)*X.m_pData[5]+b3*y0)*(1+ds)*cos(dB)/fx;
	  b1=(sqrt(R)*X.m_pData[1]+b3*x0+b2*fx*tan(dB))/fx;

	  tanF = -a3/c3;
	  sinO = -b3;
	  tanK = b1/b2; 

	  double F,O,K;
	  F=atan(tanF);
	  O=asin(sinO);
	  K=atan(tanK);
      
	  //double XS,YS,ZS;
      double B[9],XL[3];
	  B[0]=X.m_pData[0];
	  B[1]=X.m_pData[1];
	  B[2]=X.m_pData[2];
	  B[3]=X.m_pData[4];
	  B[4]=X.m_pData[5];
	  B[5]=X.m_pData[6];
	  B[6]=X.m_pData[8];
	  B[7]=X.m_pData[9];
	  B[8]=X.m_pData[10];

	  XL[0]=-X.m_pData[3];
	  XL[1]=-X.m_pData[7];
	  XL[2]=-1;
      
	  CMatrix B1,XYS,invB1,XL1;
	  B1.InitMatrix(B,3,3);
	  XL1.InitMatrix(XL,3,1);
	  B1.GetInverseMatrix(invB1); 
	  XYS=invB1*XL1;



	  FILE *fp;
	  fp=fopen("C:\\Documents and Settings\\Administrator\\桌面\\200532590141李涛\\ADAM\\DLTresult.txt","w+");
      fprintf(fp,"外方位线元素：%lf     %lf     %lf\n外方位角元素：%lf     %lf     %lf\n",XYS.m_pData[0],XYS.m_pData[1],XYS.m_pData[2],F,O,K);
      fprintf(fp,"l系数：\n%lf     %lf     %lf      %lf     %lf     %lf     %lf     %lf      %lf     %lf     %lf\n",X.m_pData[0],X.m_pData[1],X.m_pData[2],X.m_pData[3],X.m_pData[4],X.m_pData[5],X.m_pData[6],X.m_pData[7],X.m_pData[8],X.m_pData[9],X.m_pData[10]);

	  fclose(fp);

	CDialog::OnOK();
}