PCL中的ICP算法(Registration模块之IterativeClosestPoint点云配准)
目录
基本介绍
详细介绍
参数输入输出
成员函数
一个例子
基本介绍
该算法在计算机图形学领域广为人知，在三维重建和点云配准这一核心应用领域之一中具有重要的应用价值。它不仅在该领域的研究和技术发展中发挥着关键作用，在SLAM等移动机器人导航系统中同样发挥着重要作用。经过几十年的发展和完善，ICP算法已经衍生出多种改进版本以适应不同场景的需求。让我们首先了解最基本形式的ICP匹配算法及其实现细节，在这些过程中所涉及的具体步骤包括：首先通过随机抽样一致性（RANSAC）方法筛选出稳定的特征点对；然后基于这些特征点对建立初始对应关系；最后利用奇异值分解（SVD）方法求解最优变换矩阵完成配准过程。其中变换矩阵的计算方法与参考文献完全一致，并均基于奇异值分解（SVD）进行处理。
在具体的实践中，一共有3个约束来终止迭代 ：
- 迭代次数默认设置为10步；
- 前后两次转换之间的差异；
- 前后两次迭代方差之间的差异。
当然我们还可以使用kdtree加速算法。
需要注意的是，在使用该算法时，请确保输入的点云必须经过预处理以消除过复杂或过多噪声的数据。这些数据可能会导致错误信息'Invalid (NaN, Inf) point coordinates given to nearestKSearch!'产生。由于各PCL版本中函数名称可能不一致，并且对应的函数数量也可能不同，请确保正确选择所需的算法实现。
ICP算法基于SVD，其大致思路 如下:
- (1) 将初始配准后的两片点云P′(经过坐标变换后的源点云)和Q，作为精配准的初始点集；
- (2) 对源点云P’中的每一点pi，在目标点云Q中寻找距离最近的对应点qi，作为该点在目标点云中的对应点，组成初始对应点对；
- (3) 初始对应点集中的对应关系并不都是正确的，错误的对应关系会影响最终的配准结果，采用方向向量阈值剔除错误的对应点对；
- (4) 计算旋转矩阵R和平移向量T，使最小，即对应点集之间的均方误差最小；
- (5) 设定某一阈值ε=dk-dk-1和最大迭代次数Nmax, 将上一步得到的刚体变换作用于源点云P′，得到新点云P”，计算P”和Q的距离误差,，如果两次迭代的误差小于阈值ε或者当前迭代次数大于Nmax，则迭代结束，否则将初始配准的点集更新为P”和Q，继续重复上述步骤，直至满足收敛条件。
详细介绍
参数输入输出
此类基于基类Registration进行继承（包含代码段\#include\langle pcl\langle registration\langle icp\langle h\rangle \rangle \rangle \rangle）），生成对象的方法非常直接。
Pcl:: IterativeClosestPoint icp
成员函数
为此无需详细讲解各类成员函数,只需列出一些关键功能及其使用方法:
- 基础功能模块: * inline voidsetInputSource (constPointCloudSourceConstPtr &cloud) 实现基础数据输入接口。
- inline void setInputTarget(constPointCloudTargetConstPtr &cloud) 核心接口中的另一个关键操作用于获取基准数据集。
- inline void setMaxCorrespondenceDistance(doubledistance_threshold)
- 最大对应距离阈值设置：该参数定义了在进行ICP配准时的最大允许配对间距。仅当源与目标之间的配对在该阈值范围内才会被纳入计算。
- 默认情况下，默认设置非常大以至于几乎所有配对都会被考虑（PCL中默认单位为米）。
- 当源与目标之间的几何差异较大时建议适当增加该阈值以提高算法鲁棒性。
ICP算法的最大对应距离设置宁可小些也不可太大。为了保证计算效率，在设置得小时应特别注意；而如果设置过大则会严重影响测量精度。
以下是对原文本的同义改写版本
一个例子
目的：基于原始点云数据结合当前RGB帧、深度图以及相机姿态信息的基础上，在融合后得到一个更新后的点云集合，并将其整合到原有的数据集中。为了提高精度，在融合前运用ICP算法实现精确对齐。
PointCloud::Ptr Tracking::JoinPointCloudToOriginal(PointCloud::Ptr &original, const cv::Mat &imRGB, const cv::Mat &imD, cv::Mat ¤tPose)
{
//Eigen::Isometry3d T;
PointCloud::Ptr currentCloud = Image2PointCloud( imRGB, imD);
PointCloud::Ptr newCloud( new PointCloud );
// 合并点云
PointCloud::Ptr output (new PointCloud());
Eigen::Matrix<float, 4, 4> T;
cv::cv2eigen(currentPose.inv(), T);
cout << "----------- currentPose = -----------" << endl;
cout << T.matrix() << endl;
pcl::transformPointCloud(*currentCloud, *output, T.matrix());
PointCloud::Ptr targetCloud (new PointCloud());
if(mpLastCloudforICP->size() != 0)
*targetCloud = *mpLastCloudforICP;
else
*targetCloud = *original;
std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
//ICP配准
static pcl::IterativeClosestPoint<PointT, PointT> icp;
PointCloud::Ptr final(new PointCloud);
pcl::search::KdTree<PointT>::Ptr treeSrc(new pcl::search::KdTree<PointT>);
pcl::search::KdTree<PointT>::Ptr treeTarget(new pcl::search::KdTree<PointT>);
treeSrc->setInputCloud(output);
treeTarget->setInputCloud(targetCloud);
icp.setSearchMethodSource(treeSrc);
icp.setSearchMethodTarget(treeTarget);
icp.setInputSource(output);
icp.setInputTarget(targetCloud);
icp.setMaxCorrespondenceDistance(30); //Be careful of this value. It has a large influence to the result of ICP.
icp.setMaximumIterations (50);
icp.setTransformationEpsilon(1e-10);
icp.setEuclideanFitnessEpsilon(0.01);
icp.align(*final);
std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
cout << "JoinPointCloudToOriginal:icp " << ttrack << endl;
//pcl::registration::DefaultConvergenceCriteria<float>::Ptr convergenceCriteria = icp.getConvergeCriteria();
std::cout << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;
*mpLastCloudforICP = *final;
*newCloud = *original + *final;
//*newCloud = *original + *output;
t1 = std::chrono::steady_clock::now();
// Voxel grid 滤波降采样
static pcl::VoxelGrid<PointT> voxel;
PointCloud::Ptr tmp( new PointCloud() );
// only reserve one point in every 2 cm^3 area
double gridsize = 0.02;
voxel.setLeafSize( gridsize, gridsize, gridsize );
voxel.setInputCloud( newCloud );
voxel.filter( *tmp );
t2 = std::chrono::steady_clock::now();
ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
cout << "JoinPointCloudToOriginal:VoxelGrid " << ttrack << endl;
return tmp;
}
代码解读参考:
基于PCL的点云配准方法是一种利用迭代最近点算法（ICP）实现精准对齐三维空间数据集合的有效工具。