rel orientationdlg.cpp

相对定向的坐标解算

// Rel OrientationDlg.cpp : implementation file
//

#include "stdafx.h"
#include "Rel Orientation.h"
#include "Rel OrientationDlg.h"

const double LIMIT = 0.00003;
#define OVER_LIMIT( var) fabs(var)>LIMIT

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

/////////////////////////////////////////////////////////////////////////////
// CAboutDlg dialog used for App About

class CAboutDlg : public CDialog
{
public:
	CAboutDlg();

// Dialog Data
	//{{AFX_DATA(CAboutDlg)
	enum { IDD = IDD_ABOUTBOX };
	//}}AFX_DATA

	// ClassWizard generated virtual function overrides
	//{{AFX_VIRTUAL(CAboutDlg)
	protected:
	virtual void DoDataExchange(CDataExchange* pDX);    // DDX/DDV support
	//}}AFX_VIRTUAL

// Implementation
protected:
	//{{AFX_MSG(CAboutDlg)
	//}}AFX_MSG
	DECLARE_MESSAGE_MAP()
};

CAboutDlg::CAboutDlg() : CDialog(CAboutDlg::IDD)
{
	//{{AFX_DATA_INIT(CAboutDlg)
	//}}AFX_DATA_INIT
}

void CAboutDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
	//{{AFX_DATA_MAP(CAboutDlg)
	//}}AFX_DATA_MAP
}

BEGIN_MESSAGE_MAP(CAboutDlg, CDialog)
	//{{AFX_MSG_MAP(CAboutDlg)
		// No message handlers
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CRelOrientationDlg dialog

CRelOrientationDlg::CRelOrientationDlg(CWnd* pParent /*=NULL*/)
	: CDialog(CRelOrientationDlg::IDD, pParent)
{
	//{{AFX_DATA_INIT(CRelOrientationDlg)
		// NOTE: the ClassWizard will add member initialization here
	//}}AFX_DATA_INIT
	// Note that LoadIcon does not require a subsequent DestroyIcon in Win32
	m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME);

}

void CRelOrientationDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
	//{{AFX_DATA_MAP(CRelOrientationDlg)
		// NOTE: the ClassWizard will add DDX and DDV calls here
	//}}AFX_DATA_MAP
}

BEGIN_MESSAGE_MAP(CRelOrientationDlg, CDialog)
	//{{AFX_MSG_MAP(CRelOrientationDlg)
	ON_WM_SYSCOMMAND()
	ON_WM_PAINT()
	ON_WM_QUERYDRAGICON()
	ON_BN_CLICKED(IDC_BUTTON_OPENLEFT, OnButtonOpenleft)
	ON_BN_CLICKED(IDC_BUTTON_OPENRIGHT, OnButtonOpenright)
	ON_BN_CLICKED(IDC_BUTTON_RELORIENTATION, OnButtonRelorientation)
	ON_BN_CLICKED(IDC_BUTTON_OUTPUT, OnButtonOutput)
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CRelOrientationDlg message handlers

BOOL CRelOrientationDlg::OnInitDialog()
{
	CDialog::OnInitDialog();

	// Add "About..." menu item to system menu.

	// IDM_ABOUTBOX must be in the system command range.
	ASSERT((IDM_ABOUTBOX & 0xFFF0) == IDM_ABOUTBOX);
	ASSERT(IDM_ABOUTBOX < 0xF000);

	CMenu* pSysMenu = GetSystemMenu(FALSE);
	if (pSysMenu != NULL)
	{
		CString strAboutMenu;
		strAboutMenu.LoadString(IDS_ABOUTBOX);
		if (!strAboutMenu.IsEmpty())
		{
			pSysMenu->AppendMenu(MF_SEPARATOR);
			pSysMenu->AppendMenu(MF_STRING, IDM_ABOUTBOX, strAboutMenu);
		}
	}

	// Set the icon for this dialog.  The framework does this automatically
	//  when the application's main window is not a dialog
	SetIcon(m_hIcon, TRUE);			// Set big icon
	SetIcon(m_hIcon, FALSE);		// Set small icon
	
	// TODO: Add extra initialization here
	
	return TRUE;  // return TRUE  unless you set the focus to a control
}

void CRelOrientationDlg::OnSysCommand(UINT nID, LPARAM lParam)
{
	if ((nID & 0xFFF0) == IDM_ABOUTBOX)
	{
		CAboutDlg dlgAbout;
		dlgAbout.DoModal();
	}
	else
	{
		CDialog::OnSysCommand(nID, lParam);
	}
}

// If you add a minimize button to your dialog, you will need the code below
//  to draw the icon.  For MFC applications using the document/view model,
//  this is automatically done for you by the framework.

void CRelOrientationDlg::OnPaint() 
{
	if (IsIconic())
	{
		CPaintDC dc(this); // device context for painting

		SendMessage(WM_ICONERASEBKGND, (WPARAM) dc.GetSafeHdc(), 0);

		// Center icon in client rectangle
		int cxIcon = GetSystemMetrics(SM_CXICON);
		int cyIcon = GetSystemMetrics(SM_CYICON);
		CRect rect;
		GetClientRect(&rect);
		int x = (rect.Width() - cxIcon + 1) / 2;
		int y = (rect.Height() - cyIcon + 1) / 2;

		// Draw the icon
		dc.DrawIcon(x, y, m_hIcon);
	}
	else
	{
		CDialog::OnPaint();
	}
}

// The system calls this to obtain the cursor to display while the user drags
//  the minimized window.
HCURSOR CRelOrientationDlg::OnQueryDragIcon()
{
	return (HCURSOR) m_hIcon;
}

void CRelOrientationDlg::OnButtonOpenleft() 
{
	CFileDialog LeftOrien(TRUE,"iop","*.iop",NULL,"内定向文件(*.iop)",NULL);
	if (LeftOrien.DoModal() != IDOK)
	{
		return ;
	}
	
	LeftOrienPath = LeftOrien.GetPathName();
	
}

void CRelOrientationDlg::OnButtonOpenright() 
{
	CFileDialog RightOrien(TRUE,"iop","*.iop",NULL,"内定向文件(*.iop)",NULL);
	if (RightOrien.DoModal() != IDOK)
	{
		return ;
	}
	
	RightOrienPath = RightOrien.GetPathName();
	
}

void CRelOrientationDlg::OnButtonRelorientation() 
{
	CFileDialog RelOrien(TRUE,"txt","*.txt",NULL,"相对定向文件(*.txt)",NULL);
	if (RelOrien.DoModal() != IDOK)
	{
		return ;
	}
	
	RelOrienPath = RelOrien.GetPathName();
	
}

void CRelOrientationDlg::OnButtonOutput() 
{
	CFileDialog fileOutDlg(TRUE,"txt","Relative Orientation.txt",OFN_HIDEREADONLY,"相对定向结果文件(*.txt)",NULL);
	if (fileOutDlg.DoModal() != IDOK)
	{
		return ;
	}

	OutputPath = fileOutDlg.GetPathName();
}

void CRelOrientationDlg::OnOK() 
{
	if (LeftOrienPath.empty() || RelOrienPath.empty() ||
		OutputPath.empty() || RelOrienPath.empty())
	{
		AfxMessageBox("输入输出参数不完整!");
		return;
	}

	double LeftOrienData[12],
		   RightOrienData[12];
	if (!Open_iopfile( LeftOrienPath, LeftOrienData) ||
		!Open_iopfile( RightOrienPath, RightOrienData) )
	{
		AfxMessageBox("数据未能读入");
		return;
	}
	
	Corr_Point *Points = NULL;
	int Num_Point = 0;
	//获取x,y坐标，单位mm
	DataPre(Points, LeftOrienData, RightOrienData,Num_Point);

	//定义变量，3个方向角和两个系数，初始值定为0
	double Var[5];
	memset(Var, 0, 5*sizeof(double) );
	double B[3];
	//Bx本应当取2号点(6个点时)的视差，此时随机取2号点
	B[0] = Points[1].L_x - Points[1].R_x;

	//五个变量的增量
	double Var_Add[5];
	for (int i=0; i<5; ++i)
	{
		Var_Add[i] = 100;
	}

	//最大运算次数
	unsigned MaxTime = 0;

	//输出
	ofstream Result(OutputPath.c_str());

	//系数A,常数项L
	double *A = new double[5*Num_Point];
	double *L = new double[Num_Point];
	//Q为协方差矩阵，X为变量
	CMatrix X, Q ;

	//AA为系数,LL为常数
    CMatrix V, AA, LL;

    //中误差
    double mid;

	//循环跳出条件，如果最大运算次数为10
	while ( (OVER_LIMIT(Var_Add[0]) || OVER_LIMIT(Var_Add[1]) || OVER_LIMIT(Var_Add[2])
		   || OVER_LIMIT(Var_Add[3])  || OVER_LIMIT(Var_Add[4]) ) && MaxTime < 10)
	{
		//法方程系数
		double FA[25];
		//常数项
		double CH[5];
		memset(FA, 0, sizeof(double)*25);
		memset(CH, 0, sizeof(double)* 5);
		int num=0;

		for (int i=0 ; i< Num_Point ; ++i)
		{
			if (Points[i].IsValid == FALSE) 
			{
				A[5*i+0]=0;
				A[5*i+1]=0;
				A[5*i+2]=0;
				A[5*i+3]=0;
				A[5*i+4]=0;
				L[i]=0;
			}
			else
			{
			num++;
			//利用微小角旋转矩阵求解
			double R_XYZ[3],
				L_XYZ[3];
			
			//计算X2,Y2,Z2; X1, Y1, Z1
			R_XYZ[0] = 1 * Points[i].R_x - Var[2]*Points[i].R_y + Var[0]*RightOrienData[10];
			R_XYZ[1] = Var[2]*Points[i].R_x + 1*Points[i].R_y + Var[1]*RightOrienData[10];
			R_XYZ[2] = Var[0]*Points[i].R_x + Var[1]*Points[i].R_y - 1 * RightOrienData[10];
			L_XYZ[0] = Points[i].L_x;
			L_XYZ[1] = Points[i].L_y;
			L_XYZ[2] = -LeftOrienData[10];
			
			//计算By,Bz,N',N,Q
			B[1] = Var[3] * B[0];
			B[2] = Var[4] * B[0];
			
			double RN = (B[0]*L_XYZ[2] - B[2]*L_XYZ[0]) / (L_XYZ[0]*R_XYZ[2] - L_XYZ[2]*R_XYZ[0]);   //N'
			double LN = (B[0]*R_XYZ[2] - B[2]*R_XYZ[0]) / (L_XYZ[0]*R_XYZ[2] - L_XYZ[2]*R_XYZ[0]);   //N
			L[i] = LN*L_XYZ[1] - RN*R_XYZ[1] - B[1];

			//计算系数
/*			double A[5];*/
			A[5*i+0] = -R_XYZ[0]*R_XYZ[1]/R_XYZ[2] * RN;
			A[5*i+1] = -(R_XYZ[2] + R_XYZ[1]*R_XYZ[1]/R_XYZ[2]) * RN;
			A[5*i+2] = R_XYZ[0] * RN;
			A[5*i+3] = B[0];
			A[5*i+4] = -R_XYZ[1]*B[0]/R_XYZ[2];

			for(int j=0; j<5; ++j)
			{
				CH[j] += A[5*i+j] * L[i];
				for(int m=0; m<5;  ++m)
				{
					FA[j*5+m] += A[5*i+j] * A[5*i+m];
				}
			}
			}
		}

		CMatrix XS ,CS;
		XS.InitMatrix(FA, 5, 5);
		CS.InitMatrix(CH, 5, 1);
		XS.GetInverseMatrix(Q);
		X = Q * CS;

    	AA.InitMatrix(A, Num_Point, 5);
    	LL.InitMatrix(L, Num_Point, 1);
    	V = AA * X - LL;
	
    	//中误差
    	mid = sqrt(*(V.GetTransposedMatrix()*V).GetDataAddress()/(num-5));
    	Result <<"第"<<MaxTime+1<< "次迭代结果: \n"
	    	   <<"phi\t" << "         omega\t" <<"         kappa\t" 
		       <<"          mu\t" <<"        v\t"<<"        单位权中误差\n" 
	    	   <<Var[0]<<" \t"<<Var[1]<<" \t"<<Var[2]<<" \t"<<Var[3]<<" \t"<<Var[4]<<" \t"<<mid
	    	   <<endl;

		for (i=0; i<Num_Point; ++i)
		{
    	    double err;
    		V.GetMember(i, 0, err); 
    		if ( fabs(err) > 2*mid)
			{
	    		Points[i].IsValid = FALSE;
			}
		}

		MaxTime++;
		for (int ii = 0; ii<5; ++ii)
		{
			X.GetMember(ii , 0, Var_Add[ii]);
			Var[ii] += Var_Add[ii];
		}
		
	}



	//各变量的误差
	double wc[5];
	Result << "5个未知数中误差:"<<endl;
	for (i=0; i<5; ++i)
	{
		Q.GetMember(i, i, wc[i]);
		Result << sqrt(wc[i])*mid <<" \t";
	}
	Result << endl;

	Result.setf(ios::fixed,ios::floatfield);          
	Result.precision(6);  
	Result << "点号   " <<"误差值   " << "是否超限"<<endl;
	for (i=0; i<Num_Point; ++i)
	{
		double err;
		V.GetMember(i, 0, err);
		string CX("否");
		if ( Points[i].IsValid == FALSE)
		{
			string CXX("是");
			CX = CXX;
		}
		Result<< Points[i].ID << "  " << err <<"     " << CX <<endl;
	}

	for (i=0; i<Num_Point; ++i)
	{
		delete Points[i].ID;
	}

	Result.close();

	//删除分配的内存
	delete []A;
	delete []L;
	delete []Points;
	CDialog::OnOK();
}

void CRelOrientationDlg::OnCancel() 
{
	
	CDialog::OnCancel();
}

BOOL CRelOrientationDlg::Open_iopfile(string lpstrpathname,double * inorientation)
{
	ifstream InnerOrien(lpstrpathname.c_str());

	//通过扩展名检查文件是否为*.iop文件
	size_t rn = lpstrpathname.rfind(".");
	string extendname(lpstrpathname.begin()+rn, lpstrpathname.end()); 
	if ( extendname.compare(".iop") != 0)
	{
		return FALSE;
	}

	char chs[300];
	int   count =0;
	while ( InnerOrien.getline(chs,300) )
	{   
		char *tmp = new char[strlen(chs)+1];
		strcpy(tmp, chs );
		char *id=strtok( tmp ," ");
		char* p=strtok(NULL," ");
		while(p)   
		{   
			if(*p)   
			{   
				inorientation[count++] = atof(p) ;   
			}   
			p=strtok(NULL," ");   
		}  
		delete tmp;
	}

	return TRUE;
}

BOOL CRelOrientationDlg::DataPre(Corr_Point *&Points,double *LOrienData,double *ROrienData, int& Num)
{
	ifstream RelOrien(RelOrienPath.c_str());

	char chs[300];
	int   count =0;
	RelOrien.getline( chs, 300);
	Num = atoi( chs);

	Points = new Corr_Point[Num];

	while ( RelOrien.getline(chs, 300) && count < Num)
	{
		int nn = 0;
		double data[4];
		char* buf = new char[strlen(chs)+1];
//		TRACE("Lenth of buf: %d", strlen(chs)+1);
		strcpy( buf, chs);
		char *id = strtok(buf, " ");
		Points[count].ID = new char[strlen(id)+1];
		strcpy(Points[count].ID, id);
		char *p = strtok(NULL, " ");
		while (p)
		{
			if (*p)
			{
				data[nn++] = atof(p);
			}
			p = strtok(NULL, " ");
		}

		//计算x,y坐标，原始的为I,J(即data中的)
		double Left[2],
			   Right[2];
		for (int i=0; i<2; ++i)
		{
			Left[i] = data[i] - LOrienData[i];
			Right[i] = data[2+i] - ROrienData[i];
		}

		Points[count].L_x = (Left[0]*LOrienData[6] + Left[1]*LOrienData[7]) * LOrienData[11];
		Points[count].L_y = (Left[0]*LOrienData[8] + Left[1]*LOrienData[9]) * LOrienData[11];
		Points[count].R_x = (Right[0]*ROrienData[6] + Right[1]*ROrienData[7]) * ROrienData[11];
		Points[count].R_y = (Right[0]*ROrienData[8] + Right[1]*ROrienData[9]) * ROrienData[11];
		Points[count].IsValid = TRUE;

		delete buf;
		count ++;
	}
	return TRUE;
}