icp registration progress bar

icp registration process is too slow. Consider adding a progress bar. First, test the usage of the progress bar on the main dialog box, which needs to be executed by multithreading.
Create a new class MyThread, header file:

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

#define WM_USER_MSG WM_USER + 1001

class MyThread
{
	public:
		// Thread callback function must be a global function
		static void Run(void *ptr);
	public:
		MyThread(void);
		~MyThread(void);
};

Source file:

#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()
{
}

Add a progress bar control on the main dialog box with id set to IDC_PROGRESS. Add another button to trigger the progress bar with id IDC_BTN_PROGRESS.
Declare a message passing function in the header file of the dialog class: lrresult onmsg (wParam WP, lParam LP); Add the following in the source file:

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;
}

In begin_ MESSAGE_ Add: on in map_ MESSAGE(WM_USER_MSG, &CFrameMatchWindowDlg::OnMsg)
Add in OnInitDialog():

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

Add in DoDataExchange:

DDX_Control(pDX, IDC_PROGRESS, m_proGress);

But it's not easy to use = = to change a way of thinking.
First put the icp matching process into multithreading to avoid program jamming.
Move the point cloud calibration part in the OnBnClickedOk method of icpdlg to a static function and make appropriate modifications:

//ICP calibration
UINT CIcpDlg::IcpMatch(LPVOID lpParam)
{
	CIcpDlg *pIcp = CIcpDlg::gIcpDlg;
	int nTargetPoint = 0;
	int nSourcePoint = 0;
	//Query the number of points in the point cloud file
	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++;//Add the last line
	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);
	//Create pcl point cloud
	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);//Point cloud Final after source registration
	cout << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << endl;
	cout << icp.getFinalTransformation() << endl;
	//Export point cloud file
	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;
	//The coordinates of icp point pairs are output to grid control
	pIcp->IcpCorrsToGrid();
	AfxMessageBox(_T("Point cloud matching completed"), NULL, NULL);
	//pIcp->OnClose();
	return 0;
}

The main modification is to change the direct reference to the members in the class into indirect reference.
The btnclickok method says:

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

Add header file of icpdlg:

	CWinThread* m_winIcpMatch = NULL;//ICP calibration multithreading
	static UINT IcpMatch(LPVOID lpParam);//ICP calibration multithreading

In this way, the program does not get stuck.
Then consider embedding the progress bar on the dialog box of icp.
Add a progress bar control to the dialog box with id set to IDC_ ICP_ PROGRESS. The variable CProgressCtrl m is declared in the header file of IcpDlg_ IcpProgress; Associate the variable to the DDX control in DoDataExchange in the source file of IcpDlg_ Control(pDX, IDC_ICP_PROGRESS, m_IcpProgress);.
Add initialization of progress bar in CIcpDlg::CIcpDlg(CWnd* pParent /*=NULL * /):

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

Next, you need to let the progress bar get the program execution progress. Because most of the time-consuming of icp registration is in the iterative part, the display of the progress bar is updated according to the current number of iterations.
In ICP Modify the computeTransformation() function in HPP, and the iteration takes place in the do while loop in this function.
Calculate the step size of the progress bar before the do while loop, float fstep = 100.0 / pdlg - > M_ nMaxIter; m_ Nmaxiter is the maximum number of iterations entered in the dialog box. Initial progress int nProgress = 0;.
Add in the do while loop:

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

In order to prevent the iteration from ending without reaching the set maximum number of iterations, add pdlg - > m after the loop body_ IcpProgress. SetPos(100); Let the progress bar jump directly to 100%.
Such a progress bar can be used. There was a problem at the beginning. I wanted to display the progress bar in a separate dialog box, and then put the implementation of the dialog box into multithreading. This will be more cumbersome. It took a whole morning to do it, and this method does not solve the problem that the interface will get stuck during the execution of the algorithm. So we should start with algorithm multithreading.

Posted by Daniel.Conaghan1 on Thu, 12 May 2022 05:46:04 +0300