点云配准经典算法ICP复现 C++ Win10 pcl1.11.1

最新推荐文章于 2024-03-07 13:51:22 发布

原创

最新推荐文章于 2024-03-07 13:51:22 发布 · 2.2k 阅读

30 ·

CC 4.0 BY-SA版权

文章标签：

#算法 #c++ #开发语言

本文分享了如何使用C++实现经典的ICP算法，包括暴力枚举与kd树计算邻近点、单位四元数法求解最小二乘，以及不同终止准则的运用。通过实例展示了算法步骤和代码，并对比了与PCL库的性能。

背景

ICP算法是点云配准(registration)领域的主流算法，在学习过程中我尝试使用C++复现了ICP算法。我参考的是经典ICP论"P. Besl, N. McKay. ‘A Method for Registration of 3-D Shapes,’ IEEE Trans. on Pattern Analysis and Machine Intel., vol. 14, no. 2, pp. 239-256,1992"。这篇论文中使用的求解最小二乘的方法为单位四元数法，建议论文和代码搭配阅读。

环境

windows10 + pcl1.11.1

思路

计算邻近点：这一步骤设置了两种模式：暴力枚举(BRUTE_FORCE)和kd树(KD_TREE)。两种模式的思路顾名思义，需要说明的是当点云规模稍大时，暴力枚举所消耗的时间是不能被接受的。
计算旋转和平移矩阵：ICP算法中求解最小二乘一般有四种，分别为单位四元数、奇异值分解、正交矩阵法和对偶四元数。我使用的是论文中提到的单位四元数法，之后也复现了常用的奇异值分解法(SVD)
对目标点云进行旋转和平移操作
重复上述三个步骤直至终止：终止准则一般有三种，一是设置迭代次数；二是设置均方差阈值，当某一迭代过程中均方差小于该阈值则认为收敛；三是设置均方差微分阈值，当某次迭代过程中均方差与上次迭代中均方差的差值小于该阈值则认为收敛。我在程序中采用的是第一和第二种准则。

代码

计算临近点

	// first step : find the clostest points
		void ComputeCorrespondence(PointTargetPtr& cloud_ICP){
			if (methodCP == BURTE_FORCE) { // 使用穷举法计算邻近点
				// clear the correspondence vector generated at last iteration
				//vector<int>().swap(Index_corr_src);
				//vector<int>().swap(Index_corr_tar);
				//vector<int>().swap(SquareError);
				for (size_t index_source = 0; index_source < cloud_source->points.size(); ++index_source) {
					Scalar minDistance = CorrThreshold;
					size_t minTargetIndex = 0;
					for (size_t index_target = 0; index_target < cloud_target->points.size(); ++index_target) {
						cout << "calculating now... index_source = " << index_source << " index_target = " << index_target << endl;
						//Scalar distance = ComputeDistance3d(
						//	cloud_source->points[index_source].x,
						//	cloud_source->points[index_source].y,
						//	cloud_source->points[index_source].z,
						//	cloud_ICP->points[index_target].x,
						//	cloud_ICP->points[index_target].y,
						//	cloud_ICP->points[index_target].z);
						Scalar test = pow(cloud_source->points[index_source].x - cloud_ICP->points[index_target].x, 2);
						float deltaX = cloud_source->points[index_s