PCL中的点云ICP配准（附源代码和数据）
本博文参考：https://blog.****.net/wokaowokaowokao12345/article/details/73741957
介绍
给定两个来自不同坐标系的三维数据点集，找到两个点集空间的变换关系，使得两个点集能统一到同一坐标系统中，即配准过程。配准的目标是在全局坐标框架中找到单独获取的视图的相对位置和方向，使得它们之间的相交区域完全重叠。对于从不同视图（views）获取的每一组点云数据，需要一个能够将它们对齐在一起的单一点云模型，从而可以应用后续处理步骤，如分割和对象重构。这篇博文就基于PCL中的ICP算法，实现不同视图点云数据的配准，并延伸相关知识。
配准模型
两个(pairwise)点云数据集的配准，即在其中一个数据上进行旋转和平移，使得他们之间的相交区域完全重合。这里输出结果是一个（4x4）刚性（rigid）变换矩阵。
两个数据集的计算步骤：
- 识别最能代表两个数据集中的场景的兴趣点（interest points）（即关键点 keypoints）
- 在每个关键点处，计算特征描述符;
- 从特征描述符集合以及它们在两个数据集中的x,y,z位置，基于特征和位置之间的相似性来估计对应关系;
- 假设数据被认为包含噪声的，并不是所有的对应关系都是有效的，所以舍弃对配准过程产生负面影响的那些负影响对应关系;
- 从剩下的一组好的对应关系中，估计一个变换行为。
关键点
点云中的特征点是一些兴趣点，如目标的转点、角点等具有明显特征（feature）的点或点集。
特征描述基于发现的关键点，提取特征组合信息并生成向量来进行比较。一致性估计 correspondence estimate给定两组数据获取相应的特征描述向量，找到相应的特征以在数据中找到重叠的部分。
舍弃糟糕的一致性估计
不是所有的一致性估计都是正确的，错误的一致性将对变换产生负影响，因此需要舍弃。
变换 transformation
步骤：
- 根据一致性评价误差度量（error metric）
- 估计变换矩阵
- 优化点结构
迭代最近点 Iterative Closest Point （ICP）
ICP算法本质上是基于最小二乘法的最优配准方法。该算法重复进行选择对应关系点对，计算最优刚体变换这一过程，直到满足正确配准的收敛精度要求。ICP是一个广泛使用的配准算法，主要目的就是找到旋转和平移参数，将两个不同坐标系下的点云，以其中一个点云坐标系为全局坐标系，另一个点云经过旋转和平移后两组点云重合部分完全重叠。ICP算法最早由Chen and Medioni，and Besl and McKay提出。
算法的输入：参考点云和目标点云，停止迭代的标准。
算法的输出：旋转和平移矩阵，即转换矩阵。
基本过程
对于目标点云中的每个点，匹配参考点云（或选定集合）中的最近点。求得使上述对应点对计算均方根（root mean square，RMS）最小的刚体变换，求得平移参数和旋转参数。使用获得的转换矩阵来转换目标点云。迭代（重新关联点），直到满足终止迭代的条件（迭代次数或误差小于阈值）。这里的误差最小，可以是相邻两次均方根差的绝对值小于某一限差。
可能存在的问题
- 算法收敛于局部最小误差。
- 噪声或异常数据可能导致算法无法收敛或错误。
- 在进行ICP算法第一步要确定一个迭代初值，选取的初值将对最后配准结果产生重要的影响，如果初值选择不合适，算法可能就会限入局部最优，使得迭代不能收敛到正确的配准结果。
源代码
#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\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 ()
{
// The point clouds we will be using
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; // Default number of ICP iterations
pcl::console::TicToc time;
time.tic ();
if (pcl::io::loadPLYFile ("fish-2.ply", *cloud_in) < 0)
{
PCL_ERROR ("Error loading cloud %s.\n");
return (-1);
}
std::cout << "\nLoaded file " << "fish-2.ply" << " (" << cloud_in->size () << " points) in " << time.toc () << " ms\n" << 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 / 8; // 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 << "Applying this rigid transformation to: cloud_in -> cloud_icp" << std::endl;
print4x4Matrix (transformation_matrix);
// 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 (iterations);
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 << "Applied " << iterations << " ICP iteration(s) in " << time.toc () << " ms" << std::endl;
if (icp.hasConverged ())
{
std::cout << "\nICP has converged, score is " << icp.getFitnessScore () << std::endl;
std::cout << "\nICP 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 (1280, 1024); // Visualiser window size
// Register keyboard callback :
viewer.registerKeyboardCallback (&keyboardEventOccurred, (void*) NULL);
// Display the visualiser
while (!viewer.wasStopped ())
{
viewer.spinOnce ();
// The user pressed "space" :
if (next_iteration)
{
// The Iterative Closest Point algorithm
time.tic ();
icp.align (*cloud_icp);
std::cout << "Applied 1 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
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");
}
else
{
PCL_ERROR ("\nICP has not converged.\n");
return (-1);
}
}
next_iteration = false;
}
return (0);
}