PCL点云库：ICP算法原理及应用

ICP算法

• 在点云数据处理过程中，往往需要将不同视角即不同参考坐标下的两组或者多组点云统一到统一坐标系下，进行点云的配准。在配准算法中，使用最多的是ICP算法。

基本思想：

• 将目标点云P与源点云Q进行比较，匹配得到最接近的点云对。
• 对这些点云对进行相关运算得到误差函数，并计算使得误差函数最小的参数R，t。其中R 为旋转矩阵，t为平移向量。
• 在此基础上，使用上一步求得的旋转矩阵R和平移矩阵 t 对目标点云P进行旋转和平移变换，的到新的对应点集P’。
• 计算P’对应点集Q的平均距离d。
• 判断收敛条件是否成立：如果d小于给定的阈值，或者迭代次数大于预设的最大迭代次数，则停止迭代计算。否则返回第2步，直到满足收敛条件为止。

在PCL库中，IterativeClosestPoint类提供了标准ICP算法的实现（The transformation is estimated based on SVD）
算法迭代结束条件有如下几个:

• 最大迭代次数：Number of iterations has reached the maximum user imposed number of iterations (via setMaximumIterations)
• 两次变化矩阵之间的差值：The epsilon (difference) between the previous transformation and the current estimated transformation is smaller than an user imposed value (via setTransformationEpsilon)
• 均方误差（MSE）：The sum of Euclidean squared errors is smaller than a user defined threshold (via setEuclideanFitnessEpsilon)

基本用法

IterativeClosestPoint<PointXYZ, PointXYZ> icp;

// Set the input source and target
icp.setInputCloud (cloud_source);
icp.setInputTarget (cloud_target);

// Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
icp.setMaxCorrespondenceDistance (0.05);
// Set the maximum number of iterations (criterion 1)
icp.setMaximumIterations (50);
// Set the transformation epsilon (criterion 2)
icp.setTransformationEpsilon (1e-8);
// Set the euclidean distance difference epsilon (criterion 3)
icp.setEuclideanFitnessEpsilon (1);

// Perform the alignment
icp.align (cloud_source_registered);
// Obtain the transformation that aligned cloud_source to cloud_source_registered
Eigen::Matrix4f transformation = icp.getFinalTransformation ();

官方文档中的实例

// icp_2.cpp : 定义控制台应用程序的入口点。
//

#include "stdafx.h"


#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>);

	// Fill in the CloudIn data
	cloud_in->width = 5;
	cloud_in->height = 1;
	cloud_in->is_dense = false;
	cloud_in->points.resize(cloud_in->width * cloud_in->height);
	for (std::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 (std::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;
	std::cout << "size:" << cloud_out->points.size() << std::endl;
	for (std::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 (std::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;
	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
	icp.setInputSource(cloud_in);
	icp.setInputTarget(cloud_out);
	pcl::PointCloud<pcl::PointXYZ> Final;
	icp.align(Final);
	std::cout << "has converged:" << icp.hasConverged() << " score: " <<
		icp.getFitnessScore() << std::endl;
	std::cout << icp.getFinalTransformation() << std::endl;

	system("pause");
	return (0);
}

运行结果

has converged 为1表示，两个点云数据之间只是简单的刚性变换，这与实际符合。

• 该实例中，源点云通过随机数生成，而目标点云由源点云作简单的刚性变换后得到，程序输出正确的转换矩阵。

参考：
How to use iterative closest point
http://pointclouds.org/documentation/tutorials/iterative_closest_point.php#iterative-closest-point
Interactive Iterative Closest Point
http://pointclouds.org/documentation/tutorials/interactive_icp.php#interactive-icp
PCL点云库：ICP算法
https://www.cnblogs.com/21207-iHome/p/6034462.html