【点云配准】迭代最近点(ICP)配准算法

文章目录

目录

文章目录

前言

一、迭代最近点(ICP)配准算法

参考文献 

二、实现代码

三、结果展示

---

前言

        点云配准（Point Cloud Registration）指的是输入两幅点云(source)和(target) ，
        输出一个变换T使得T(Ps)和T(Pt)的重合程度尽可能高；Ps和Pt是源点云和目标点云中的对应点。
        变换T可以是刚性的(rigid)，也可以不是，一般只考虑刚性变换，即变换只包括旋转、平移。
        点云配准可以分为粗配准（Coarse Registration）和精配准（Fine Registration）两步。
        粗配准指的是在两幅点云之间的变换完全未知的情况下进行较为粗糙的配准，目的主要是为精配准提供较好的变换初值；
        精配准则是给定一个初始变换，进一步优化得到更精确的变换。
        目前应用最广泛的点云精配准算法是迭代最近点算法（Iterative Closest Point, ICP）及各种变种 ICP 算法。

---

一、迭代最近点(ICP)配准算法

        1992 年，Paul J.Besl 等人首次提出了 ICP 算法，以点集对点集（PSTPS）配准方法为 基础，阐述了一种曲面拟合算法，用于解决基于自由形态曲面的配准问题。其基本迭代流程分为如下四个过程：

1）搜索最近点；

2）通过配准点对进行 SVD(Singular Value Decomposition)分解；

3）计算姿态变换矩阵 Rt；

4）求解误差，通过阈值判断是否继续迭代。

ICP 算法流程见算法 1。表中，Q、P 分别为参考点与待配准点三维点云坐标集合，q、p 代表集合中子集，为某一参考点或待配准点三维坐标； 、 为初始三维旋转矩阵与三维平移矩阵，用于初始位姿变换，R、t 为最终输出三维旋转矩阵与三维平移矩阵；Reproject 过程代表通过 R、t 对 P 进行位姿变换；SearchNN 过程代表搜索最近点，通过求解两点之间 欧氏距离寻求最小值得出；EstimateTrans 过程为求解最终姿态变换矩阵 R、t。

参考文献 

[1]石峰源,张春明,姜丽辉,周琦,潘迪.采用主成分分析的ICP算法优化与验证[J/OL].激光与光电子学进展:1-17

二、实现代码

代码如下（示例）：

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/console/time.h>   // 控制台计算时间
#include <boost/thread/thread.hpp>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/voxel_grid.h>
#include <vtkAutoInit.h>
VTK_MODULE_INIT(vtkRenderingOpenGL);

void  visualize_registration(pcl::PointCloud<pcl::PointXYZ>::Ptr& source,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& target, pcl::PointCloud<pcl::PointXYZ>::Ptr& regist);
using namespace std;

//------------------------------------------------------------------------------------------------//
void visualizeCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& source_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& output_cloud)
//void visualizeCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud,
//    pcl::PointCloud<pcl::PointXYZ>::Ptr& source_cloud, pcl::PointCloud<pcl::PointXYZ> output_cloud)
{
    // 初始化点云可视化对象
    boost::shared_ptr<pcl::visualization::PCLVisualizer>viewer(new pcl::visualization::PCLVisualizer("显示点云"));
    viewer->setBackgroundColor(255, 255, 255);
    // 对目标点云着色可视化 (red).
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target_cloud, 255, 0, 0);
    viewer->addPointCloud<pcl::PointXYZ>(target_cloud, target_color, "target cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target cloud");
    // 对源点云着色可视化 (blue).
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> input_color(source_cloud, 0, 0, 255);
    viewer->addPointCloud<pcl::PointXYZ>(source_cloud, input_color, "input cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "input cloud");
    // 对转换后的源点云着色 (green)可视化.
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> output_color(output_cloud, 0, 255, 0);
    viewer->addPointCloud<pcl::PointXYZ>(output_cloud, output_color, "output cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "output cloud");
    // 等待直到可视化窗口关闭
    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(1000));
    }
}

//PCL ICP算法实现点云精配准
#include <pcl/registration/icp.h> // icp算法
void ICP(pcl::PointCloud<pcl::PointXYZ>::Ptr& target,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& source)
{

    pcl::console::TicToc time;
    time.tic();
    //--------------------初始化ICP对象--------------------
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    //----------------------icp核心代码--------------------
    icp.setInputSource(source);            // 源点云
    icp.setInputTarget(target);            // 目标点云
    icp.setTransformationEpsilon(1e-10);   // 为终止条件设置最小转换差异
    icp.setMaxCorrespondenceDistance(1);  // 设置对应点对之间的最大距离（此值对配准结果影响较大）。
    icp.setEuclideanFitnessEpsilon(0.0001);  // 设置收敛条件是均方误差和小于阈值，停止迭代；
    icp.setMaximumIterations(1000);           // 最大迭代次数
    icp.setUseReciprocalCorrespondences(true);//设置为true,则使用相互对应关系
                                              // 计算需要的刚体变换以便将输入的源点云匹配到目标点云

    pcl::PointCloud<pcl::PointXYZ>::Ptr icp_result(new pcl::PointCloud<pcl::PointXYZ>);
    icp.align(*icp_result);
    pcl::transformPointCloud(*source, *icp_result, icp.getFinalTransformation());
    Eigen::Matrix4f icp_trans;
    icp_trans = icp.getFinalTransformation();
//    std::cout << icp_trans << endl;

    pcl::PointCloud<pcl::PointXYZ>::Ptr icp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    icp.align(*icp_cloud);
    cout << "Applied " << 35 << " ICP iterations in " << time.toc() << " ms" << endl;
    cout << "\nICP has converged, score is " << icp.getFitnessScore() << endl;
    cout << "变换矩阵：\n" << icp.getFinalTransformation() << endl;
    // 使用创建的变换对为输入源点云进行变换
    pcl::transformPointCloud(*source, *icp_cloud, icp.getFinalTransformation());

    visualize_registration(target,source,icp_cloud);
}

---

三、结果展示

 总结：从结果看匹配失败，算法陷入局部最优解。所以在进行ICP匹配之前，最好使用粗配准算法，为ICP算法提供一个良好的初始解，不仅可以防止ICP算法陷入局部最优，还可以加速迭代过程。