1. Introduction
Two point sets, source and target, the target is unchanged, the source is rotated (Rotation) and translated (Translation) and even scale (Scale) transformation is added, so that the transformed source point set coincides with the target point set as much as possible. The process is called point set registration.
However, the accuracy of the ICP method is relatively poor. If an original point cloud is moved a certain distance in the X direction (or the original point cloud is down-sampled), then the registration score will be very different, and the obtained The rotation and translation matrices are also quite different.
2. Basic principles
Suppose Qi,i=1,2,3,⋯Qi,i=1,2,3,⋯ represent the first point set, Pi,i=1,2,3,⋯Pi,i=1,2, 3,... represents the second point set.
ICP registration process:
1. Calculate each point in the original point cloud P for the closest point in the matching point cloud Q
2. Calculate the average minimum distance of the corresponding points and calculate the RT matrix
3. Perform RT conversion on the original point cloud P to get P2
4. Calculate the average minimum distance between P2 and Q, and continue to iterate.
Tip: The requirement of the objective function here can also be the number of iterations
Identification steps:
1. Find the points of interest or key points in the two point cloud data sets respectively
2. Computing Feature Descriptors
3. Calculate p1(x,y,z) p2(x,y,z) respectively to calculate the correlation between features and positions, and calculate by iterative testing
4. Before calculation, the noise on the point cloud must be removed. ICP is a violent iterative method, and the calculation content is all points in the point cloud.
5. Always find a best matching feature from the feature points to calculate the rotation and translation matrix
Iterative Closest Point (ICP)
PCL study notes two: Registration (ICP algorithm)_rabbif's blog-CSDN blog
ICP uses the optimal registration method of the least squares method. This algorithm repeatedly performs the correspondence between points, calculates a rigid transformation, and knows the accuracy that meets the convergence.
// 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 is based on SVD, SVD is singular value decomposition
basic process
For each point in the target point cloud, match the closest point in the reference point cloud (or selected set).
Obtain the rigid body transformation that minimizes the root mean square (RMS) of the above corresponding point pairs, and obtain the translation parameters and rotation parameters.
Use the obtained transformation matrix to transform the target point cloud.
Iterate (re-associate points) until the condition for terminating the iteration is met (number of iterations or error less than a threshold). The error here is the smallest, and it can be that the absolute value of the root mean square difference of two adjacent times is less than a certain tolerance.
3. Code:
add header file
#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();