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%。
这样的进度条就可以使用了。一开始的思路就有问题，想要把进度条放到单独的对话框里显示，再把对话框的实现放到多线程里。这样就会比较繁琐，用了一上午也没做出来，而且这种办法并没有解决算法执行过程中会让界面卡住的问题。所以还是要从算法多线程着手。