ICP算法进行点云配准

一、算法原理

1.配准

给定两个来自不同坐标系的三维数据点集，找到两个点集空间的变换关系，使得两个点集能统一到同一坐标系统中，即配准过程。

2.ICP（Iterative Closest Point，迭代最近邻点）

ICP本质上是基于最小二乘法的最优配准方法，精度高，不需要提取特征点；但是需要在icp使用之前两点云已经完成粗配准，否则容易陷入局部最优。该算法重复进行选择对应关系点对，计算最优刚体变换这一过程，直到满足正确配准的收敛精度要求。ICP是一个广泛使用的配准算法，主要目的就是找到旋转和平移参数，将两个不同坐标系下的点云，以其中一个点云坐标系为全局坐标系，另一个点云经过旋转和平移后两组点云重合部分完全重叠。算法只适用于刚体配准；算法不适用于部分重叠点云的配准。
算法的输入：参考点云和目标点云，停止迭代的标准。
算法的输出：旋转和平移矩阵，即转换矩阵。

ICP算法原理
假设点云{Q}为目标点云（参考点云），{P}为源点云（待配准的点云）， p i ( i ∈ 1 , 2 , . . . N )， q i 是{E}中与 pi距离最近的点。​
我们需要计算从{P }到{Q }的RT变换矩阵，即旋转矩阵R和平移矩阵T。如果变换参数是准确的，那么点云{P }中的每一个点 p i ，经过变换后应该与点云{Q}中的点 q i 完全重合，即： qi = R pi + T 。但由于有噪声的存在，不可能所有点都完全重合，所以我们定义目标函数：

使目标函数最小的R，T即为所求变换参数。F其实就是参考点云{Q } 与 已经进行R,T矩阵空间变换的{P’} 之间的平均距离。
计算方法：首先，对{P}中的每个点 pi ，寻找其在{Q}中的最近点 qi （利用kd树最近邻查找算法可实现），组成一一对应的点对。计算两组点云的质心，分别记为 u p , u q ：

对两组点云进行去质心，得到：

构建矩阵H：

对H矩阵进行SVD分解：

（SVD（奇异值分解）是一种常用的矩阵重要特征计算工具，我们只是借助它计算RT矩阵，在此暂不必深究），得到R与T：

得到R，T矩阵以后，用其对待配准空间进行空间变换得到新的点集，并代入目标函数：

若如果新的变换点集与参考点集满足两点集的平均距离小于某一给定阈值，则停止迭代计算，否则新的变换点集作为新的{P_i }继续迭代，直到达到目标函数的要求。

总结ICP算法：
(1)计算{P}中的每一个点在{Q}点集中的对应近点；
(2)求得使上述对应点对平均距离最小的刚体变换，求得平移参数和旋转参数；
(3)对{P}使用上一步求得的平移和旋转矩阵进行空间变换，得到新的变换点集{P’}；
(4)如果新的变换点集与参考点集满足两点集的平均距离小于某一给定阈值，或者迭代次数达到设定的最大值，则停止迭代计算，否则新的变换点集作为新的{P}继续迭代，直到达到目标函数的要求。

可能存在的问题
(1)算法收敛于局部最小误差。
(2)噪声或异常数据可能导致算法无法收敛或错误。
(3)在进行ICP算法第一步要确定一个迭代初值，选取的初值将对最后配准结果产生重要的影响，如果初值选择不合适，算法可能就会限入局部最优，使得迭代不能收敛到正确的配准结果。
二、源代码（PCL1.1.1+Visual Studio 2019）

#include <pcl/registration/ia_ransac.h>//点云的ransac算法头文件
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>//pcd输入输出头文件
#include <pcl/registration/icp.h>//点云icp算法头文件
#include <pcl/visualization/pcl_visualizer.h>//点云可视化头文件
#include <time.h>
#include <boost/thread.hpp>
typedef pcl::PointXYZ PointT; //重定义pcl::PointXYZ为PointT
typedef pcl::PointCloud<PointT> PointCloud; //重定义pcl::PointCloud<PointT>为PointCloud

//点云可视化
void visualize_pcd(PointCloud::Ptr pcd_src,
    PointCloud::Ptr pcd_tgt,
    PointCloud::Ptr pcd_final)
{
    pcl::visualization::PCLVisualizer viewer("registration Viewer");
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(pcd_src, 0, 255, 0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(pcd_tgt, 255, 0, 0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> final_h(pcd_final, 0, 0, 255);
    viewer.addPointCloud(pcd_src, src_h, "source cloud");    //source绿色
    viewer.addPointCloud(pcd_tgt, tgt_h, "tgt cloud");     //target红色
    //viewer.addPointCloud (pcd_final, final_h, "final cloud");  //final蓝色

    while (!viewer.wasStopped())
    {
        viewer.spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }
}

int main(int argc, char** argv)
{
    //加载点云文件
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_src_o(new pcl::PointCloud<pcl::PointXYZ>);//原点云，待配准
    pcl::io::loadPCDFile("scan11.pcd", *cloud_src_o);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tgt_o(new pcl::PointCloud<pcl::PointXYZ>);//目标点云
    pcl::io::loadPCDFile("scan22.pcd", *cloud_tgt_o);
    clock_t start = clock();

    //icp配准
    pcl::PointCloud<pcl::PointXYZ>::Ptr icp_result(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;

    // 最大迭代次数
    icp.setMaximumIterations(50);
    // 两次变化矩阵之间的差值
    icp.setTransformationEpsilon(1e-10);
    // 均方误差
    icp.setEuclideanFitnessEpsilon(0.01);

    icp.setInputSource(cloud_src_o);//录入source点云
    icp.setInputTarget(cloud_tgt_o);//录入target点云
    pcl::PointCloud<pcl::PointXYZ> final_cloud;
    icp.align(final_cloud);//最终配准结果
    clock_t end = clock();
    cout << "total time: " << (double)(end - start) / (double)CLOCKS_PER_SEC << " s" << endl;//输出配准所用时间
    std::cout << "ICP has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl;
    Eigen::Matrix4f icp_trans;
    icp_trans = icp.getFinalTransformation();
    std::cout << endl << "R,T=" << icp_trans << endl;//输出R、T
    //使用创建的变换对未过滤的输入点云进行变换
    pcl::transformPointCloud(*cloud_src_o, *icp_result, icp_trans);

    visualize_pcd(cloud_src_o, cloud_tgt_o, icp_result);
    return (0);
}

二、运行结果及分析

用来配准的两组点云数据下载：点这里

配准数据1：

room_scan1.pcd

room_scan2.pcd

配准结果：

配准数据2：

bunny1.pcd

bunny2.pcd：

配准结果：