一、简介

ICP算法详解——我见过最清晰的解释_负壹的博客-CSDN博客

两个点集，source和target，target不变，source经过旋转（Rotation）和平移（Translation）甚至加上尺度（Scale）变换，使得变换后的source点集尽量和target点集重合，这个变换的过程就叫点集配准。

但是ICP的使用方法精度比较差，假如将一个原始点云在X方向上移动一定的距离（或者对原始点云进行降采样），然后进行配准分数也会有很大的差异，并且得到的旋转平移矩阵也差异比较大。

二、基本原理

假设用Qi,i=1,2,3,⋯Qi,i=1,2,3,⋯表示第一个点集，Pi,i=1,2,3,⋯Pi,i=1,2,3,⋯表示第二个点集。

ICP配准过程：

1.计算原始点云P中的每个点对于在匹配点云Q的最近点

2、计算对应点的平均最小距离，计算R T 矩阵

3、对原始点云P进行RT转换得到P2

4、计算P2与Q之间的平均最小距离，继续迭代。

提示：这里的目标函数的要求，也可以是迭代次数

20130518113251019 (752×606)

识别步骤：

1、分别找到两个点云数集中的兴趣点或者关键点

2、计算特征描述符

3、分别计算p1(x,y,z） p2(x,y,z)计算特征和位置之间的相关性，通过迭代测试来计算

4、计算之前一定要去除掉点云上面的噪声，ICP是暴力迭代的方法，计算内容是全部点云中的点

5、从特征点总找到一个匹配最好的特征计算出旋转平移矩阵

迭代最近点 Iterative Closest Point （ICP）

PCL学习笔记二：Registration (ICP算法)_rabbif的博客-CSDN博客

ICP采用最小二乘法的最优配准方法，此算法重复进行点之间的对应关系，计算出一个刚性变换，知道满足收敛的精度。

	// ICP配准pcl::IterativeClosestPoint<pcl::PointXYZ,pcl::PointXYZ>icp; // 创建ICP对象  用来ICP配准//icp.setMaxCorrespondenceDistance(0.1);  // icp.setTransformationEpsilon(0.1);  // 前一个变换矩阵和当前变换矩阵的差异小于阈值时，就认为已经收敛了，是一条收敛条件icp.setEuclideanFitnessEpsilon(0.01);  // 还有一条收敛条件是均方误差和小于阈值， 停止迭代icp.setMaximumIterations(10);  // 最大迭代次数icp.setInputCloud(cloud_tr);icp.setInputTarget(cloud_in);  // 输入目标点云icp.align(*cloud_icp);  //配准后的点云

transformation estimation是基于SVD的， SVD是奇异值分解

基本过程

对于目标点云中的每个点，匹配参考点云（或选定集合）中的最近点。
求得使上述对应点对计算均方根（root mean square，RMS）最小的刚体变换，求得平移参数和旋转参数。
使用获得的转换矩阵来转换目标点云。
迭代（重新关联点），直到满足终止迭代的条件（迭代次数或误差小于阈值）。这里的误差最小，可以是相邻两次均方根差的绝对值小于某一限差。

三、代码：

添加头文件

#define BOOST_TYPEOF_EMULATION  //要加在#include <pcl/registration/icp.h>前
#include <pcl/registration/icp.h>   //ICP配准类相关头文件

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);// 原始点云pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_icp(new pcl::PointCloud<pcl::PointXYZ>);  // ICP 输出点云pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tr(new pcl::PointCloud<pcl::PointXYZ>);   // 匹配点云string strfilepath = "C:\\Users\\Albert\\Desktop\\Binary.pcd";if (-1 == pcl::io::loadPCDFile(strfilepath, *cloud_in)) {cout << "error input!" << endl;return -1;}cout << cloud_in->points.size() << endl;string strfilepath2 = "C:\\Users\\Albert\\Desktop\\Binary2.pcd";//if (-1 == pcl::io::loadPCDFile(strfilepath2, *cloud_icp)) {//	cout << "error input!" << endl;//	return -1;//}pcl::PointXYZ point;for (int i = 0; i < cloud_in->size();i++){point.x= cloud_in->points[i].x + 10;point.y = cloud_in->points[i].y;point.z = cloud_in->points[i].z;cloud_icp->push_back(point);}cout << cloud_icp->points.size() << endl;pcl::io::savePCDFileBinaryCompressed(strfilepath2,*cloud_icp);int  iterations = 3;// 默认的ICP的迭代次数*cloud_tr = *cloud_icp;// ICP配准pcl::IterativeClosestPoint<pcl::PointXYZ,pcl::PointXYZ>icp; // 创建ICP对象  用来ICP配准//icp.setMaxCorrespondenceDistance(0.1);  // icp.setTransformationEpsilon(0.1);  // 前一个变换矩阵和当前变换矩阵的差异小于阈值时，就认为已经收敛了，是一条收敛条件icp.setEuclideanFitnessEpsilon(0.01);  // 还有一条收敛条件是均方误差和小于阈值， 停止迭代icp.setMaximumIterations(10);  // 最大迭代次数icp.setInputCloud(cloud_tr);icp.setInputTarget(cloud_in);  // 输入目标点云icp.align(*cloud_icp);  //配准后的点云if (icp.hasConverged())//icp.hasConverged ()=1（true）输出变换矩阵的适合性评估{std::cout << "\nICP has converged, score is " << icp.getFitnessScore() << std::endl;}else{PCL_ERROR("\nICP has not converged.\n");return (-1);}cout<<icp.getFinalTransformation();