hjdlg.cpp

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

// HJDlg.cpp : implementation file
//

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

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

/////////////////////////////////////////////////////////////////////////////
// CHJDlg dialog


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


void CHJDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
	//{{AFX_DATA_MAP(CHJDlg)
	DDX_Text(pDX, IDC_EDIT01, m_NFW);
	DDX_Text(pDX, IDC_EDIT02, m_XD);
	DDX_Text(pDX, IDC_EDIT03, m_KZD);
	DDX_Text(pDX, IDC_EDIT04, m_WFWCZ);
	//}}AFX_DATA_MAP
}


BEGIN_MESSAGE_MAP(CHJDlg, CDialog)
	//{{AFX_MSG_MAP(CHJDlg)
	ON_BN_CLICKED(IDC_BUTTON01, OnButton0)
	ON_BN_CLICKED(IDC_BUTTON02, OnButton2)
	ON_BN_CLICKED(IDC_BUTTON03, OnButton4)
	ON_BN_CLICKED(IDC_BUTTON04, OnButton5)
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CHJDlg message handlers

void CHJDlg::OnButton0() 
{
	// 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_NFW=FileDlg.GetPathName();
	}
	UpdateData(FALSE);	
}

void CHJDlg::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_XD=FileDlg.GetPathName();
	}
	UpdateData(FALSE);	
}

void CHJDlg::OnButton4() 
{
	// 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 CHJDlg::OnButton5() 
{
	// 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_WFWCZ=FileDlg.GetPathName();
	}
	UpdateData(FALSE);	
}
void CHJDlg::OnOK() 
{
	// TODO: Add extra validation here
	
	CString Mdata=m_NFW;
		if(Mdata=="")
		{
			return;
		}
	CString Idata=m_XD;
		if(Idata=="")
		{
			return;
		}
	CString Jdata=m_KZD;
		if(Jdata=="")
		{
			return;
		}
	CString Kdata=m_WFWCZ;
		if(Kdata=="")
		{
			return;
		}
    double xs,ys,zs,F,O,K,x0,y0,f,pixelsize,k1,k2,p1,p2,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(Mdata);
	for (i=0;i<8;i++)
	{
		in>>x0>>y0>>f>>pixelsize>>k1>>k2>>p1>>p2;
	}
	in.close();
	
	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();

	in.open(Kdata);
	for (i=0;i<6;i++)
	{
		in>>xs>>ys>>zs>>F>>O>>K;
	}
	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 a1,a2,a3,b1,b2,b3,c1,c2,c3;
	double A[23*2*6],L[23*2];
	a1=cos(F)*cos(K)-sin(F)*sin(O)*sin(K);
	a2=-cos(F)*sin(K)-sin(F)*sin(O)*cos(K);
	a3=-sin(F)*cos(O);
	b1=cos(O)*sin(K);
	b2=cos(O)*cos(K);
	b3=-sin(O);
	c1=sin(F)*cos(K)+cos(F)*sin(O)*sin(K);
	c2=-sin(F)*sin(K)+cos(F)*sin(O)*cos(K);
	c3=cos(F)*cos(O);
	
	for(i=0;i<23;i++)
	{
		L[i+0]=-f*(a1*(PPKZDx[i]-xs)+b1*(PPKZDy[i]-ys)+c1*(PPKZDz[i]-zs))/(a3*(PPKZDx[i]-xs)+b3*(PPKZDy[i]-ys)+c3*(PPKZDz[i]-zs));
		L[i+1]=-f*(a2*(PPKZDx[i]-xs)+b2*(PPKZDy[i]-ys)+c2*(PPKZDz[i]-zs))/(a3*(PPKZDx[i]-xs)+b3*(PPKZDy[i]-ys)+c3*(PPKZDz[i]-zs));
	}
	
	for (i=0;i<23;i++)
	{
		A[i+0]=(a1*f+a3*L[i+0])/a3*(PPKZDx[i]-xs)+b3*(PPKZDy[i]-ys)+c3*(PPKZDz[i]-zs);
		A[i+1]=(b1*f+b3*L[i+0])/a3*(PPKZDx[i]-xs)+b3*(PPKZDy[i]-ys)+c3*(PPKZDz[i]-zs);
        A[i+2]=(c1*f+c3*L[i+0])/a3*(PPKZDx[i]-xs)+b3*(PPKZDy[i]-ys)+c3*(PPKZDz[i]-zs);
        A[i+3]=(a2*f+a3*L[i+1])/a3*(PPKZDx[i]-xs)+b3*(PPKZDy[i]-ys)+c3*(PPKZDz[i]-zs);
        A[i+4]=(b2*f+b3*L[i+1])/a3*(PPKZDx[i]-xs)+b3*(PPKZDy[i]-ys)+c3*(PPKZDz[i]-zs);
        A[i+5]=(c2*f+c3*L[i+1])/a3*(PPKZDx[i]-xs)+b3*(PPKZDy[i]-ys)+c3*(PPKZDz[i]-zs);
        A[i+6]=L[i+1]*sin(O)-((L[i+0]*(L[i+0]*cos(K)-L[i+1]*sin(K))/f)+f*cos(K))*cos(O);
        A[i+7]=-f*sin(K)-L[i+0]*(L[i+0]*sin(K)+L[i+1]*cos(K))/f;
		A[i+8]=L[i+1];
		A[i+9]=L[i+0]*sin(O)-((L[i+1]*(L[i+0]*cos(K)-L[i+1]*sin(K))/f)-f*sin(K))*cos(O);
        A[i+10]=-f*cos(K)-L[i+1]*(L[i+0]*sin(K)+L[i+1]*cos(K))/f;
		A[i+11]=-L[i+0];
	}

    CMatrix AA,AT,ATA,invATA;
	AA.InitMatrix(A,46,6);
	AA.GetTransposedMatrix(AT);
	ATA=AT*AA;
	ATA.GetInverseMatrix(invATA); 
	
    CMatrix CL;
	CL.InitMatrix(L,46,1);

	CMatrix X;
	X=invATA*AT*CL;

    double XS,YS,ZS,PHI,OMIGA,KAPA;
	XS=xs+X.m_pData[0];
	YS=ys+X.m_pData[1];
	ZS=zs+X.m_pData[2];
	PHI=F+X.m_pData[3];
	OMIGA=O+X.m_pData[4];
	KAPA=K+X.m_pData[5];

    FILE *fp;
	fp=fopen("C:\\Documents and Settings\\Administrator\\桌面\\200532590141李涛\\ADAM\\HJresult.txt","w+");
    fprintf(fp,"外方位线元素：%lf     %lf     %lf\n外方位角元素：%lf     %lf     %lf\n",XS,YS,ZS,PHI,OMIGA,KAPA);
	fclose(fp);

	CDialog::OnOK();
}