PCL_ ICP迭代最近点

ICP  (Iterative Closest Point)  迭代最近点。

参考：https://zhuanlan.zhihu.com/p/35893884   对ICP问题引入解释的很到位了，反手一个赞

https://blog.csdn.net/Asher_zheng/article/details/97491462 有示例，有自己写的拓展的，待细看

https://www.cnblogs.com/sddai/p/6129437.html 对ICP的具体过程解释的很OK

https://blog.csdn.net/u010696366/article/details/8941938  这是 巨详细全面的一个博客[大拇指]

https://blog.csdn.net/shao918516/article/details/105062787 这是很详细的刚体变换的介绍

---

ICP是一种点云匹配算法，就是一个点云与经过刚体变换后的点云，不断缩小他们之间的距离，计算出他们之间的旋转和平移矩阵 <R,t> ,从而实现进行匹配等，经常用到！

常用的求解 R 和 T 的方法有：

1. SVD（singular value decomposition）奇异值分解
2. 非线性优化

---

pcl源代码示例： 

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
int
 main (int argc, char** argv)
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);

  // 填入点云数据
  cloud_in->width    = 5;
  cloud_in->height   = 1;
  cloud_in->is_dense = false;
  cloud_in->points.resize (cloud_in->width * cloud_in->height);
  for (size_t i = 0; i < cloud_in->points.size (); ++i)
  {
    cloud_in->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud_in->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud_in->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
  }
  std::cout << "Saved " << cloud_in->points.size () << " data points to input:"
      << std::endl;

  for (size_t i = 0; i < cloud_in->points.size (); ++i) std::cout << "    " <<
      cloud_in->points[i].x << " " << cloud_in->points[i].y << " " <<
      cloud_in->points[i].z << std::endl;
  *cloud_out = *cloud_in;
  //这里相当于对填充的点云进行了一个简单的刚体变换，每个x都加了0.7
  std::cout << "size:" << cloud_out->points.size() << std::endl;
  for (size_t i = 0; i < cloud_in->points.size (); ++i)
    cloud_out->points[i].x = cloud_in->points[i].x + 0.7f;
  std::cout << "Transformed " << cloud_in->points.size () << " data points:"
      << std::endl;
  for (size_t i = 0; i < cloud_out->points.size (); ++i)
    std::cout << "    " << cloud_out->points[i].x << " " <<
      cloud_out->points[i].y << " " << cloud_out->points[i].z << std::endl;

  //创建一个IterativeClosestPoint实例，使用的奇异值分解
  pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
  icp.setInputCloud(cloud_in);
  icp.setInputTarget(cloud_out);
  //新定义的Final用来存储应用ICP算法之后的结果
  pcl::PointCloud<pcl::PointXYZ> Final;
  icp.align(Final);
  //如果变换前后点云正确Align的话（即变换点云通过刚性变换之后几乎和变换后点云完全重合）
  //则 icp.hasConverged() = 1 (true)，然后输出fitness得分和其他一些相关信息。
  std::cout << "has converged:" << icp.hasConverged() << " score: " <<
  icp.getFitnessScore() << std::endl;
  std::cout << icp.getFinalTransformation() << std::endl;//获得最后的变换矩阵
 return (0);
}

此外还可以通过设置setMaximumIterations来确定初次配准时的迭代次数，当初次应用ICP算法时，值可以设置的稍微大一点，以使两个点云的点可以尽可能的接近。

实验结果：

hasconverged=1，说明匹配出是同一个点云 ，下面4*4的矩阵就是他的位姿变换矩阵。

---

实例：

第二个参考链接里面有个例子，跑了一下，我认为无论是他改过的还是改之前的代码都有些问题，我也改了改。

主要的改动是：添加了键盘响应事件的循环，原来的只能响应一次，也只能迭代一次；并且设置了最大迭代次数，并将此作为循环的限定条件。

同时需要注意的是，点击空格时，光标的位置是在可视化窗口上，而不是在命令行窗口点击空格。

可视化窗口迭代次数的更新，需要先设置成空格，之后再重新复制迭代的数字。

#include <iostream>
#include <string>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/time.h>   // TicToc

typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

bool next_iteration = false;

void
print4x4Matrix(const Eigen::Matrix4d & matrix)
{
	printf("Rotation matrix :\n");
	printf("    | %6.3f %6.3f %6.3f | \n", matrix(0, 0), matrix(0, 1), matrix(0, 2));
	printf("R = | %6.3f %6.3f %6.3f | \n", matrix(1, 0), matrix(1, 1), matrix(1, 2));
	printf("    | %6.3f %6.3f %6.3f | \n", matrix(2, 0), matrix(2, 1), matrix(2, 2));
	printf("Translation vector :\n");
	printf("t = < %6.3f, %6.3f, %6.3f >\n", matrix(0, 3), matrix(1, 3), matrix(2, 3));

}

void
keyboardEventOccurred(const pcl::visualization::KeyboardEvent& event,
	void* nothing)
{
	if (event.getKeySym() == "space" && event.keyDown())
		next_iteration = true;
}

int
main(int argc,
	char* argv[])
{
	PointCloudT::Ptr cloud_in(new PointCloudT);  // Original point cloud  
	PointCloudT::Ptr cloud_tr(new PointCloudT);  // Transformed point cloud  
	PointCloudT::Ptr cloud_icp(new PointCloudT);  // ICP output point cloud 

	int iterations = 1;
	// Checking program arguments
	if (argc < 2)
	{
		printf("Usage :\n");
		printf("\t\t%s file.ply number_of_ICP_iterations\n", argv[0]);
		PCL_ERROR("Provide one ply file.\n");
		return (-1);
	}

	int iternum = 1;  // Default number of ICP iterations
	if (argc > 2)
	{
		//If the user passed the number of iteration as an argument
		iternum = atoi(argv[2]);
		std::cout << "最大迭代次数为" <<iternum<<"次！\n"<< std::endl;
		if (iterations < 1)
		{
			PCL_ERROR("Number of initial iterations must be >= 1\n");
			return (-1);
		}
	}

	pcl::console::TicToc time;
	time.tic();
	if (pcl::io::loadPLYFile(argv[1], *cloud_in) < 0)
	{
		PCL_ERROR("Error loading cloud %s.\n", argv[1]);
		return (-1);
	}
	float rat = 0.001f;//convert to meter(m)
	for (size_t i = 0; i < cloud_in->points.size(); ++i)
	{
		cloud_in->points[i].x *= rat;
		cloud_in->points[i].y *= rat;
		cloud_in->points[i].z *= rat;
	}
	std::cout << "Loaded file " << argv[1] << " (" << cloud_in->size() << " points) in " << time.toc() << " ms" << std::endl;

	// Defining a rotation matrix and translation vector
	Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity();
	// A rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)
	double theta = M_PI / 4;  // The angle of rotation in radians
	transformation_matrix(0, 0) = cos(theta);
	transformation_matrix(0, 1) = -sin(theta);
	transformation_matrix(1, 0) = sin(theta);
	transformation_matrix(1, 1) = cos(theta);
	// A translation on Z axis (0.4 meters)
	transformation_matrix(2, 3) = 0.4;

	// Display in terminal the transformation matrix
	std::cout << "\nApplying this rigid transformation to: cloud_in -> cloud_icp" << std::endl;
	print4x4Matrix(transformation_matrix);
	std::cout << std::endl;
	// Executing the transformation
	pcl::transformPointCloud(*cloud_in, *cloud_icp, transformation_matrix);
	*cloud_tr = *cloud_icp;  // We backup cloud_icp into cloud_tr for later use

	// The Iterative Closest Point algorithm
	time.tic();
	pcl::IterativeClosestPoint<PointT, PointT> icp;
	icp.setMaximumIterations(iternum);
	icp.setInputSource(cloud_icp);
	icp.setInputTarget(cloud_in);
	icp.align(*cloud_icp);
	icp.setMaximumIterations(1);  // We set this variable to 1 for the next time we will call .align () function


	std::cout << "\n\nApplied " << iterations << " ICP iteration in " << time.toc() << " ms" << std::endl;
	if (icp.hasConverged())
	{
		std::cout << "ICP has converged, score is " << icp.getFitnessScore() << std::endl;
		std::cout << "ICP transformation " << iterations << " : cloud_icp -> cloud_in" << std::endl;
		transformation_matrix = icp.getFinalTransformation().cast<double>();
		print4x4Matrix(transformation_matrix);
	}
	else
	{
		PCL_ERROR("\nICP has not converged.\n");
		return (-1);
	}


	// Visualization
	pcl::visualization::PCLVisualizer viewer("ICP demo");
	// Create two vertically separated viewports
	int v1(0);
	int v2(1);
	viewer.createViewPort(0.0, 0.0, 0.5, 1.0, v1);
	viewer.createViewPort(0.5, 0.0, 1.0, 1.0, v2);

	// The color we will be using
	float bckgr_gray_level = 0.0;  // Black
	float txt_gray_lvl = 1.0 - bckgr_gray_level;

	// Original point cloud is white
	pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_in_color_h(cloud_in, (int)255 * txt_gray_lvl, (int)255 * txt_gray_lvl,
		(int)255 * txt_gray_lvl);
	viewer.addPointCloud(cloud_in, cloud_in_color_h, "cloud_in_v1", v1);
	viewer.addPointCloud(cloud_in, cloud_in_color_h, "cloud_in_v2", v2);

	// Transformed point cloud is green
	pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_tr_color_h(cloud_tr, 20, 180, 20);
	viewer.addPointCloud(cloud_tr, cloud_tr_color_h, "cloud_tr_v1", v1);

	// ICP aligned point cloud is red
	pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_icp_color_h(cloud_icp, 180, 20, 20);
	viewer.addPointCloud(cloud_icp, cloud_icp_color_h, "cloud_icp_v2", v2);

	// Adding text descriptions in each viewport
	viewer.addText("White: Original point cloud\nGreen: Matrix transformed point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_1", v1);
	viewer.addText("White: Original point cloud\nRed: ICP aligned point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_2", v2);

	std::stringstream ss;
	ss << iterations;
	std::string iterations_cnt = "ICP iterations = " + ss.str();
	viewer.addText(iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt", v2);

	// Set background color
	viewer.setBackgroundColor(bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v1);
	viewer.setBackgroundColor(bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v2);

	// Set camera position and orientation
	viewer.setCameraPosition(-3.68332, 2.94092, 5.71266, 0.289847, 0.921947, -0.256907, 0);
	viewer.setSize(960, 540);  // Visualiser window size
	std::cout << "第1次迭代完成！\n\n" << std::endl;

								 // Register keyboard callback :
	//viewer.registerKeyboardCallback(&keyboardEventOccurred, (void*)NULL);

	// Display the visualiser
	while (!viewer.wasStopped())
	{
		viewer.spinOnce();
		// The user pressed "space" :
		if (iterations <= iternum)
		{
			viewer.registerKeyboardCallback(&keyboardEventOccurred, (void*)NULL);
			if (next_iteration)
			{
				// The Iterative Closest Point algorithm
				time.tic();
				icp.align(*cloud_icp);
				iterations += 1;
				std::cout << "Applied " << iterations << " ICP iteration in " << time.toc() << " ms" << std::endl;

				if (icp.hasConverged())
				{
					//printf("\033[11A");  // Go up 11 lines in terminal output.
					//printf("\nICP has converged, score is %+.0e\n", icp.getFitnessScore());
					//std::cout << "\nICP transformation " << iterations << " : cloud_icp -> cloud_in" << std::endl;
					//transformation_matrix *= icp.getFinalTransformation().cast<double>();  // WARNING /!\ This is not accurate! For "educational" purpose only!
					//print4x4Matrix(transformation_matrix);  // Print the transformation between original pose and current pose

					std::cout << "ICP has converged, score is " << icp.getFitnessScore() << std::endl;
					std::cout << "ICP transformation " << iterations << " : cloud_icp -> cloud_in" << std::endl;
					transformation_matrix = icp.getFinalTransformation().cast<double>();
					print4x4Matrix(transformation_matrix);

					ss.str("");//设置成空的
					ss << iterations;
					std::string iterations_cnt = "ICP iterations = " + ss.str();
					viewer.updateText(iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt");
					viewer.updatePointCloud(cloud_icp, cloud_icp_color_h, "cloud_icp_v2");
					std::cout << "第" << iterations << "次迭代完成！\n\n"  << std::endl;
				}
				else
				{
					PCL_ERROR("\nICP has not converged.\n");
					return (-1);
				}
			}
			next_iteration = false;
		}	
	}
	return (0);
}

结果：

在命令行输入点云文件以及最大迭代次数后，显示如下，矩阵为原点云变为另一点云进行的刚体变换。

下图为第一次迭代信息：

之后每点击一次空格键，更新一次信息，同时，第一次迭代相当于粗配准，变化较大且花费时间较多。之后每次都是一点点的变，花费的时间也逐渐变短。 

以下为迭代1次，16次，30次的结果，可以看到，随着迭代次数的增加，他们之间的距离越来越小，越来越贴合。

---

日常嘴碎： 

害，我以为下面这个代码分分钟搞定，没想到堂堂小菜鸡竟然改来改去试来试去搞了老半天。不过也是第一次用这种交互的键盘响应，奥利给！

据说5月6号之后就要开学了，我们学校我们还是第一批，啧，45%的想去，55%的不想去。[微笑]

但无论开不开，也希望疫情早日过去，还有不明所以的外国友人们保持理智不要再乱甩锅骂别的国家哦。

今日刷博又被柠檬，于是我作诗一首,大家一起品一品。

——【人间四月芳菲尽，科研路上柠檬开】。Fine~