icp配準進度條

icp配準過程太慢了，考慮加個進度條。先在主對話方塊上測試一下進度條的用法，需要用多執行緒來執行。
新建一個類MyThread，標頭檔案：

#pragma once
#include <process.h>
#include <Windows.h>

#define WM_USER_MSG WM_USER + 1001

class MyThread
{
	public:
		// 執行緒回撥函式必須是全域性函式
		static void Run(void *ptr);
	public:
		MyThread(void);
		~MyThread(void);
};

原始檔：

#include "stdafx.h"
#include "MyThread.h"
void MyThread::Run(void *ptr)
{
	HWND hWnd = (HWND)ptr;
	for (int i = 0; i<105; ++i)
	{
		::PostMessage(hWnd, WM_USER_MSG, WPARAM(i), LPARAM(0));
		Sleep(100);
	}
	AfxMessageBox(_T("done"));
	_endthread();
}

MyThread::MyThread()
{
}
MyThread::~MyThread()
{
}

在主對話方塊上新增一個進度條控制元件，id設定為IDC_PROGRESS。再新增一個按鈕用於觸發進度條，id為IDC_BTN_PROGRESS。
在對話方塊類的標頭檔案宣告一個訊息傳遞的函式：LRESULT OnMsg(WPARAM wp, LPARAM lp);在原始檔中如下新增：

void CFrameMatchWindowDlg::OnBnClickedBtnProgress()
{
	_beginthread(&MyThread::Run, 0, this->GetSafeHwnd());
}
	

LRESULT CFrameMatchWindowDlg::OnMsg(WPARAM wp, LPARAM lp)
{
	CProgressCtrl *prog = (CProgressCtrl *)GetDlgItem(IDC_PROGRESS);
	prog->SetPos((int)wp);
	return 1L;
}

在BEGIN_MESSAGE_MAP裡新增：ON_MESSAGE(WM_USER_MSG, &CFrameMatchWindowDlg::OnMsg)
OnInitDialog()裡新增：

CDialogEx::OnInitDialog();
CProgressCtrl *prog = (CProgressCtrl *)GetDlgItem(IDC_PROGRESS);
	prog->SetRange(0, 100);

DoDataExchange裡新增：

DDX_Control(pDX, IDC_PROGRESS, m_proGress);

但是並不好用= =換一種思路。
先把icp匹配的過程放到多執行緒裡，避免程式卡住。
把icpdlg的OnBnClickedOk方法裡的點雲校準部分移動到一個靜態函式裡，並做適當修改：

//ICP校準
UINT CIcpDlg::IcpMatch(LPVOID lpParam)
{
	CIcpDlg *pIcp = CIcpDlg::gIcpDlg;
	int nTargetPoint = 0;
	int nSourcePoint = 0;
	//查詢點雲檔案內點的數量
	CFrameMatchWindowDlg *pFMWDlg = CFrameMatchWindowDlg::gFrameMatchWindowDlg;
	char* strTargetFile = new char[80];
	FILE* pfTargetData;
	strcpy(strTargetFile, pFMWDlg->m_strTargetCloudFile);
	fopen_s(&pfTargetData, strTargetFile, "r");
	if (pfTargetData == NULL)
		return 0;
	while (!feof(pfTargetData))
		if (fgetc(pfTargetData) == '\n')
			nTargetPoint++;
	nTargetPoint++;//加上最後一行
	cout << "target cloud contains " << nTargetPoint << " points." << endl;
	fclose(pfTargetData);
	char* strSourceFile = new char[80];
	FILE* pfSourceData;
	strcpy(strSourceFile, pFMWDlg->m_strSourceCloudFile);
	fopen_s(&pfSourceData, strSourceFile, "r");
	if (pfSourceData == NULL)
		return 0;
	while (!feof(pfSourceData))
		if (fgetc(pfSourceData) == '\n')
			nSourcePoint++;
	nSourcePoint++;
	cout << "source cloud contains " << nSourcePoint << " points." << endl;
	fclose(pfSourceData);
	//建立pcl點雲
	PointCloud<PointXYZ>::Ptr cloud_target(new PointCloud<PointXYZ>(nTargetPoint, 1));
	PointCloud<PointXYZ>::Ptr cloud_source(new PointCloud<PointXYZ>(nSourcePoint, 1));
	// Fill in the cloud data
	{
		FILE* pfSourceData;
		fopen_s(&pfSourceData, strSourceFile, "r");
		if (pfSourceData == NULL) return 0;
		uint64_t nTimestamp = 0;
		for (auto& point : *cloud_source)
			fscanf(pfSourceData, "%f,%f,%f,%f,%lld,", &point.x, &point.y, &point.z, &point.data[3], &nTimestamp);
		fclose(pfSourceData);
		delete[] strSourceFile;
		strSourceFile = NULL;
	}
	{
		FILE* pfTargetData;
		fopen_s(&pfTargetData, strTargetFile, "r");
		if (pfTargetData == NULL) return 0;
		uint64_t nTimestamp = 0;
		for (auto& point : *cloud_target)
			fscanf(pfTargetData, "%f,%f,%f,%f,%lld,", &point.x, &point.y, &point.z, &point.data[3], &nTimestamp);
		fclose(pfTargetData);
		delete[] strTargetFile;
		strTargetFile = NULL;
	}
	IterativeClosestPoint<PointXYZ, PointXYZ> icp;
	icp.setInputSource(cloud_source);
	icp.setInputTarget(cloud_target);
	PointCloud<PointXYZ> Final;
	Eigen::Matrix4f Guess;
	Guess(0) = 1.0; Guess(4) = 0.0; Guess(8) = 0.0; Guess(12) = 0.0;
	Guess(1) = 0.0; Guess(5) = 1.0; Guess(9) = 0.0; Guess(13) = 0.0;
	Guess(2) = 0.0; Guess(6) = 0.0; Guess(10) = 1.0; Guess(14) = 0.0;
	Guess(3) = 0.0; Guess(7) = 0.0; Guess(11) = 0.0; Guess(15) = 1.0;

	icp.align(Final, Guess);//將source進行配準後的點雲Final
	cout << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << endl;
	cout << icp.getFinalTransformation() << endl;
	//輸出點雲檔案
	FILE* pfIcpMatchedCloud;
	USES_CONVERSION;
	char* strMatchedFileName = new char[80];
	strcpy(strMatchedFileName, T2A(pIcp->m_strIcpMatchFilePath));
	fopen_s(&pfIcpMatchedCloud, strMatchedFileName, "w");
	if (pfIcpMatchedCloud == NULL) return 0;
	for (int i = 0; i < Final.size(); i++)
		fprintf(pfIcpMatchedCloud, "%f,%f,%f,%.1f,\n", Final.at(i).x, Final.at(i).y, Final.at(i).z, Final.at(i).data[3]);
	FILE* pfIcpTargetFile;
	fopen_s(&pfIcpTargetFile, pFMWDlg->m_strTargetCloudFile, "r");
	if (pfIcpTargetFile == NULL) return 0;
	float nReflection;
	uint64_t lTimestamp;
	float dTargetPointX, dTargetPointY, dTargetPointZ;
	while (fscanf_s(pfIcpTargetFile, "%f,%f,%f,%f,%lld,", &dTargetPointX, &dTargetPointY, &dTargetPointZ, &nReflection, &lTimestamp) != EOF)
		fprintf(pfIcpMatchedCloud, "%f,%f,%f,%.1f,\n", dTargetPointX, dTargetPointY, dTargetPointZ, nReflection);
	fclose(pfIcpTargetFile);
	fclose(pfIcpMatchedCloud);
	delete[] strMatchedFileName;
	strMatchedFileName = NULL;
	//icp點對座標輸出到grid control內
	pIcp->IcpCorrsToGrid();
	AfxMessageBox(_T("點雲匹配完成"), NULL, NULL);
	//pIcp->OnClose();
	return 0;
}

主要修改的地方就是把直接引用類內成員的地方改成間接引用。
btnclickok方法裡寫：

m_winIcpMatch = AfxBeginThread(IcpMatch, NULL, THREAD_PRIORITY_NORMAL, 0, 0, NULL);

icpdlg的標頭檔案新增：

	CWinThread* m_winIcpMatch = NULL;//ICP校準多執行緒
	static UINT IcpMatch(LPVOID lpParam);//ICP校準多執行緒

這樣可以實現程式不卡死。
然後考慮把進度條鑲嵌在icp的對話方塊上。
對話方塊新增一個進度條控制元件，id設為IDC_ICP_PROGRESS。IcpDlg的標頭檔案裡宣告變數CProgressCtrl m_IcpProgress;在IcpDlg的原始檔裡的DoDataExchange裡把變數關聯到控制元件DDX_Control(pDX, IDC_ICP_PROGRESS, m_IcpProgress);。
在CIcpDlg::CIcpDlg(CWnd* pParent /*=NULL*/)裡新增進度條的初始化：

m_IcpProgress.SetRange(0, 100);
	m_IcpProgress.SetStep(1);
	m_IcpProgress.SetPos(0);

接下來需要讓進度條獲取程式執行進度。因為icp配準的絕大部分耗時是在迭代部分，所以根據當前迭代次數更新進度條的顯示。
在icp.hpp裡的computeTransformation()函式裡進行修改，迭代就發生在這個函式裡的do-while迴圈。
在do-while迴圈前面計算進度條的步長，float fStep = 100.0 / pDlg->m_nMaxIter;m_nMaxIter是在對話方塊輸入的最大迭代次數。初始進度int nProgress = 0;。
do-while迴圈裡新增：

nProgress += fStep;
pDlg->m_IcpProgress.SetPos(nProgress);

為了防止迭代次數沒有達到設定的最大次數就結束迭代，在迴圈體的後面加上pDlg->m_IcpProgress.SetPos(100);讓進度條直接跳到100%。
這樣的進度條就可以使用了。一開始的思路就有問題，想要把進度條放到單獨的對話方塊裡顯示，再把對話方塊的實現放到多執行緒裡。這樣就會比較繁瑣，用了一上午也沒做出來，而且這種辦法並沒有解決演算法執行過程中會讓介面卡住的問題。所以還是要從演算法多執行緒著手。