搞懂ICP（最近邻迭代）配准算法 - 2

你搜到这找资料是希望能找到讲的明白的，希望我能把一个知识点讲明白

 

前文我们已经有做配准的工具和数据（1. 三维点云配准的简介、工具和数据集），接下来讲配准里最经典的算法，迭代最近邻算法，英文Iterative Closest Point（ICP）。提到Iterative有没有想到代码里出现频繁的i++大兄弟？是个循环呀。我看了很多介绍ICP算法的，上来都是介绍一次配准算法公式（p=Rq+T，详见：https://mp.weixin.qq.com/s/V6ZZ42cFQC3SxpIhcroB7A或者高博的SLAM十四讲），一次配准算法只是ICP的其中一个模块好吧，ICP为啥叫最近邻迭代的背景没讲，ICP里有啥也没讲，不说明白靠人猜啊，导致新手碰到一次配准计算公式以外的东西，一头雾水。

ICP其实是一种算法框架，一种算法思路，一个循环底下包括一次匹配关系估计、一次匹配关系的约束（可能是多个约束的叠加）、一次配准计算、一次源数据和配准矩阵的变换、以及两个中止条件的判断，见下图。

那问题来了，为什么会有ICP算法，存在即是合理呀？

我们先看一次匹配计算公式，正常来讲，黄色的的source方块和绿色的target方块正确估计匹配关系（黑色的线），一次匹配计算公式配准就结束了，通常我们做的标定就是人为的把匹配关系对应起来，计算的二者的旋转平移矩阵即可（SVD只是求解旋转平移矩阵的一种方法）。

那么在实际应用过程中，计算机还没聪明到准确知道黄色source方块和绿色的target方块之间的匹配关系，可能算成下图左边图示的模样（计算机通过FLANN查找最近邻的匹配关系），另外实际应用时，三维点云数据是有噪声的，这时候连人工匹配它们之间的关系都不好使了。

所以就有了通过多次迭代循环来近似求解，源点云循环一次动一下，下一次再估计一下最近邻的匹配关系，再来求解一次位姿变换，最终只要邻近的两次位姿变换太小了（是ICP陷入局部最优解的根本原因），或者超过设定的循环次数就跳出迭代了。

按照ICP算法框架，部分ICP的PCL代码如下，另也可看看ICP的PCL官网教程,但是整个框架也不全面：

	do
	{
		double aRMSE = 0;	//当前迭代的均方根误差
		double mRMSE = 0;
		double rmseDiff = 0;

		pre_transformation = transformSVD;

		// 估计匹配关系
		pcl::registration::CorrespondenceEstimation est;
		pcl::CorrespondencesPtr correspondences(new pcl::Correspondences());
		est.setInputSource(input_transformed);
		est.setInputTarget(cloud_tgt);
		est.determineReciprocalCorrespondences(*correspondences, 0.05);


		// 剔除匹配关系，可以添加更多的模块
                pcl::registration::CorrespondenceRejectorSampleConsensus rejector_sac;
		pcl::CorrespondencesPtr correspondences_filtered(new pcl::Correspondences());
		rejector_sac.setInputSource(input_transformed);
		rejector_sac.setInputTarget(cloud_tgt);
		rejector_sac.setInlierThreshold(0.1); // distance in m, not the squared distance
		rejector_sac.setMaximumIterations(1000000);
		rejector_sac.setRefineModel(false);
          rejector_sac.setInputCorrespondences(correspondences_result_rej_one_to_one);
		rejector_sac.getCorrespondences(*correspondences_filtered);
		correspondences.swap(correspondences_filtered);

		//计算迭代前对应关系的均方根误差
		if (iterances = 0)
		{
			for (size_t i = 0; i < correspondences_filtered->size(); i++)
			{
				double after_distance = 0;
				after_distance = sqrt(pow((input_transformed->points[correspondences_filtered->at(i).index_match].x - cloud_tgt->points[correspondences_filtered->at(i).index_query].x), 2) +
					pow((input_transformed->points[correspondences_filtered->at(i).index_match].y - cloud_tgt->points[correspondences_filtered->at(i).index_query].y), 2) +
					pow((input_transformed->points[correspondences_filtered->at(i).index_match].z - cloud_tgt->points[correspondences_filtered->at(i).index_query].z), 2));
				mRMSE += pow(after_distance, 2);

			}
			fRMSE = sqrt(mRMSE / count);
		}

                //设置SVD匹配算法模块
		pcl::registration::TransformationEstimationSVD testSVD;
		testSVD.estimateRigidTransformation(*input_transformed, *cloud_tgt, *correspondences_filtered, transformSVD);

		// 更新源点云
		pcl::transformPointCloud(*input_transformed, *input_transformed, transformSVD);
                // 更新变换矩阵
		best_transformation = transformSVD * best_transformation;

		// 计算配准后的精度
		std::cout << "第" << iterances + 1 << "次迭代配准精度RMSE" << endl;
		for (int i = 0; i < correspondences_filtered->size(); i++)
		{

			double after_distance = 0;
			after_distance = sqrt(pow((input_transformed->points[correspondences_filtered->at(i).index_match].x - cloud_tgt->points[correspondences_filtered->at(i).index_query].x), 2) +
				pow((input_transformed->points[correspondences_filtered->at(i).index_match].y - cloud_tgt->points[correspondences_filtered->at(i).index_query].y), 2) +
				pow((input_transformed->points[correspondences_filtered->at(i).index_match].z - cloud_tgt->points[correspondences_filtered->at(i).index_query].z), 2));
			mRMSE += pow(after_distance, 2);

		}
		aRMSE = sqrt(mRMSE / count);

                //计算匹配前后的均方根
		rmseDiff = abs(aRMSE - fRMSE);

                //判断计算匹配前后的均方根是否满足用户设定的阈值
		if (rmseDiff < 精度阈值)
		{
			break;
		}

		fRMSE = aRMSE;

		++iterator;

	} 
//判断迭代是否超过用户设定的阈值
while (iterator < 迭代次数阈值);

 

ICP一般用作配准里的精确配准，为了节省时间，一般会为ICP提供一个粗配准输出的初值，一般用于刚性物体的配准。

ICP根据输入的数据类型分为点点、面面、点面、点和颜色、点和尺寸之间各种组合组成各种icp版本，还有的icp根据匹配关系估计的不同设计也有不同的版本，有兴趣可以详聊，或者后期再出一节博客，敬请期待。

柔性ICP配准算法比刚性ICP算法复杂（比刚性ICP好玩），输出的是多个节点的变换位姿，但套路差不多。刚柔配准我的闲鱼号都有视频，欢迎交流。

 

20210119更新：

https://www.cnblogs.com/21207-iHome/p/6038853.html

icp综述：https://max.book118.com/html/2017/0629/118979365.shtm