3.常见的点云配准方法（C++）

本文参考了点云配准算法的综述_采样一致性配准-CSDN博客

? ? ? ? 点云配准实际上可以理解为：通过计算得到完美的坐标变换，将处于不同视角下的点云数据经过旋转平移等刚性变换统一整合到指定坐标系之下的过程。再通俗一点讲：进行配准的两个点云，它们彼此之间可以通过旋转平移等这种位置变换完全重合，因此这两个点云属于刚性变换即形状大小是完全一样的，只是坐标位置不一样而已。点云配准就是求出两个点云之间的坐标位置变换关系。
因此，点云配准基本的输入输出是：

• 两个刚性变换的点云：源点云（source）以及目标点云（target）形状、大小相同；

• 点云配准得到一个旋转平移变换矩阵RT，该矩阵表示了这两个点云的位置变换关系，即通过RT可以将源点云变换到目标点云的位置，使二者能够重合。

点云配准总体上分为粗配准和精配准，下面对两种方法分别介绍

一、点云的粗配准

? ? ? ? 粗略配准是在源点云与目标点云完全不知道任何初始相对位置的情况下，所进行的配准方法。该方法的主要目的是在初始条件未知的情况下，快速估算一个大致的点云配准矩阵。整个计算过程要求比较高的计算速度，对于计算结果的精确度则不做过高的要求。常见的粗略配准算法的思路包括了：基于局部特征描述的方法、基于全局搜索策略以及通过统计学概率等方法。
1. 基于局部特征描述的方法是通过提取source与target的邻域几何特征，通过几何特征快速确定二者之间的点对的对应关系，再计算此关系进而获得变换矩阵。而点云的几何特征包括了很多种，比较常见的即为快速点特征直方图（Fast Point Feature Histgrams，简称FPFH）。

2. 基于全局搜索策略的代表算法是采样一致性算法（Sample Consensus Initial Alignment，简称SAC_IA），该算法在source与target之间随机选取几何特征一致的点组成点对。通过计算对应点对的变换关系，得到最优解。

3. 正态分布算法（NDT）利用统计学概率的方法，根据点云正态分布情况，确定了对应点对从而计算target与source之间的变换关系。

PCL的采样一致性算法代码：

/* 计算点云的FPFH特征 */
fpfhFeature::Ptr compute_fpfh_feature(pcl::PointCloud<pcl::PointXYZI>::Ptr input_cloud, pcl::search::KdTree<pcl::PointXYZI>::Ptr tree) {
    pointnormal::Ptr point_normal (new pointnormal);
    pcl::NormalEstimation<pcl::PointXYZI,pcl::Normal> est_normal;
    est_normal.setInputCloud(input_cloud);
    est_normal.setSearchMethod(tree);
    est_normal.setKSearch(10);
    est_normal.compute(*point_normal);
    fpfhFeature::Ptr fpfh (new fpfhFeature);
    pcl::FPFHEstimationOMP<pcl::PointXYZI,pcl::Normal, pcl::FPFHSignature33> est_fpfh;
    est_fpfh.setNumberOfThreads(8);
    est_fpfh.setInputCloud(input_cloud);
    est_fpfh.setInputNormals(point_normal);
    est_fpfh.setSearchMethod(tree);
    est_fpfh.setKSearch(10);
    est_fpfh.compute(*fpfh);
    return fpfh;
}

void coarseMatching(pcl::PointCloud<pcl::PointXYZI>::Ptr source, pcl::PointCloud<pcl::PointXYZI>::Ptr target) {
    pcl::PointCloud<pcl::PointXYZI>::Ptr sourceGrid(new pcl::PointCloud<PointType>);
    voxel_grid.setLeafSize(3,3,3);
    voxel_grid.setInputCloud(source);
    voxel_grid.filter(*sourceGrid);             
    pcl::PointCloud<pcl::PointXYZI>::Ptr targetGrid(new pcl::PointCloud<PointType>);
    voxel_grid_tar.setLeafSize(3,3,3);
    voxel_grid_tar.setInputCloud(target);
    voxel_grid_tar.filter(*targetGrid);
    fpfhFeature::Ptr source_fpfh = compute_fpfh_feature(sourceGrid, tree);
    fpfhFeature::Ptr target_fpfh = compute_fpfh_feature(targetGrid, tree);
    sac_ia.setInputSource(sourceGrid);
    sac_ia.setSourceFeatures(source_fpfh);
    sac_ia.setInputTarget(targetGrid);
    sac_ia.setTargetFeatures(target_fpfh);
    pcl::PointCloud<pcl::PointXYZI>::Ptr coarse(new pcl::PointCloud<PointType>);
    // sac_ia.setNumberOfSamples(20);//设置每次迭代计算中使用的样本数量（可省）,可节省时间
    sac_ia.setCorrespondenceRandomness(6);//设置计算协方差时选择多少近邻点，该值越大，协方差越精确，但是计算效率越低.(可省)
    /* 粗略配准结果，存储在矩阵transformation_Preprocessing之中 */
    sac_ia.align(*coarse, transformation_Preprocessing);
}

二、点云的精配准

? ? ? 精确配准是利用已知的初始变换矩阵，通过迭代最近点算法（ICP算法）等计算得到较为精确的解。ICP算法通过计算source与target对应点距离，构造旋转平移矩阵RT，通过RT对source变换，计算变换之后的均方差。若均方差满足阈值条件，则算法结束。否则则继续重复迭代直至误差满足阈值条件或者迭代次数终止。因此，ICP算法具有以下条件：

1. 配准结果精确度较高，是一种精确配准算法；
2. 对初始矩阵要求严格，差的初始矩阵严重影响算法性能，甚至会造成局部最优的情况；
?

PCL的ICP算法代码：

void icpMatching(pcl::PointCloud<pcl::PointXYZI>::Ptr source, pcl::PointCloud<pcl::PointXYZI>::Ptr target) {
    icp.setInputSource(source);
    icp.setInputTarget(target);
    icp.setTransformationEpsilon(1e-12);
    icp.setEuclideanFitnessEpsilon(1e-5);
    icp.setMaximumIterations(5000);
    /* init_transform为粗略配准算法得到的初始矩阵 */
    icp.align(*pointcloud_Result, init_transform);
    transformation_ICP = icp.getFinalTransformation();
}