PCL 点云配准点到面ICP算法（精配准）

一、概述

1.1原理

1.2实现步骤

1.3应用场景

二、代码实现

2.1关键函数

[2.1.1 法线估计和拼接函数](#2.1.1 法线估计和拼接函数)

[2.1.2 ICP配准算法](#2.1.2 ICP配准算法)

[2.1.3 点云可视化函数](#2.1.3 点云可视化函数)

2.2完整代码

三、实现效果

PCL点云算法汇总及实战案例汇总的目录地址链接：

PCL点云算法与项目实战案例汇总（长期更新）

一、概述

ICP (Iterative Closest Point) 算法是一种用于点云配准的常见方法，它通过迭代来寻找两个点云之间的刚体变换关系。**本文实现了基于KD树加速的点到面ICP算法，该算法在精度配准中比点到点ICP效果更好。**此算法通过使用点云的法线信息来提高点云配准的精度。

1.1原理

ICP算法通过以下步骤进行配准：

初始对齐：给定两个点云，源点云和目标点云，ICP在每次迭代中计算每个点与其最近的对应点之间的距离。

最小化误差：计算源点云和目标点云之间的最小二乘误差，更新源点云的刚体变换。

**迭代更新：**重复上述步骤，直到收敛条件满足为止（误差小于指定阈值或迭代次数超过限制）。

在点到面的ICP中，每个源点的最近对应点不仅包括空间坐标，还包括法线信息，使得误差最小化过程更为准确。

1.2实现步骤

**加载点云数据：**从PCD文件中加载源点云和目标点云。

**计算法线：**为源点云和目标点云计算法线信息，并拼接到点云中。

**ICP点到面配准：**利用点到面ICP算法进行精确配准，输出最终的刚体变换矩阵。

**结果可视化：**可视化配准前后的点云，显示配准效果。

1.3应用场景

精密工业制造中的3D扫描物体配准。

机器人定位与导航中的环境点云对齐。

建筑模型重建中的多视角数据融合。

二、代码实现

2.1关键函数

2.1.1 法线估计和拼接函数

cpp 复制代码

void cloud_with_normal(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointNormal>::Ptr& cloud_normals)
{
    // OMP并行计算法线，加速法线估计
    pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    
    // 使用KD树进行近邻搜索，优化法线估计速度
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
    n.setNumberOfThreads(10);  // 设置OpenMP的线程数
    n.setInputCloud(cloud);
    n.setSearchMethod(tree);
    n.setKSearch(10);  // 使用K近邻搜索，寻找10个最近点
    n.compute(*normals);  // 计算法线

    // 拼接点云和法线信息，生成包含法线的点云
    pcl::concatenateFields(*cloud, *normals, *cloud_normals);
}

2.1.2 ICP配准算法

cpp 复制代码

// 点到面的ICP配准算法封装
void perform_icp_registration(pcl::PointCloud<pcl::PointNormal>::Ptr& source_with_normals,
    pcl::PointCloud<pcl::PointNormal>::Ptr& target_with_normals, pcl::PointCloud<pcl::PointNormal>::Ptr& p_icp_cloud)
{
    pcl::IterativeClosestPointWithNormals<pcl::PointNormal, pcl::PointNormal> p_icp;
    p_icp.setInputSource(source_with_normals);
    p_icp.setInputTarget(target_with_normals);
    p_icp.setTransformationEpsilon(1e-10);   // 设置转换终止条件
    p_icp.setMaxCorrespondenceDistance(10);  // 设置最大对应距离
    p_icp.setEuclideanFitnessEpsilon(0.001); // 设置收敛条件
    p_icp.setMaximumIterations(35);          // 设置最大迭代次数
    p_icp.align(*p_icp_cloud);

    std::cout << "\nICP has converged, score is " << p_icp.getFitnessScore() << std::endl;
    std::cout << "变换矩阵：\n" << p_icp.getFinalTransformation() << std::endl;
}

2.1.3 点云可视化函数

cpp 复制代码

void  visualize_registration(pcl::PointCloud<pcl::PointXYZ>::Ptr& source,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& target, pcl::PointCloud<pcl::PointXYZ>::Ptr& icp)
{
    // 创建PCL可视化对象
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("配准结果"));
    
    // 创建两个视口用于显示配准前后的点云
    int v1 = 0;
    int v2 = 1;
    viewer->createViewPort(0, 0, 0.5, 1, v1);  // 左视口
    viewer->createViewPort(0.5, 0, 1, 1, v2);  // 右视口
    viewer->setBackgroundColor(0, 0, 0, v1);   // 左视口背景为黑色
    viewer->setBackgroundColor(0.05, 0, 0, v2); // 右视口背景为深灰色
    viewer->addText("Raw point clouds", 10, 10, "v1_text", v1);  // 左视口标题
    viewer->addText("Registed point clouds", 10, 10, "v2_text", v2);  // 右视口标题
    
    // 为源点云、目标点云和配准后的点云添加颜色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(source, 255, 0, 0);  // 源点云红色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(target, 0, 0, 255);  // 目标点云蓝色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transe(icp, 255, 0, 0);    // 配准后的点云红色
    
    // 将点云添加到对应的视口中
    viewer->addPointCloud(source, src_h, "source cloud", v1);  // 左视口显示原始点云
    viewer->addPointCloud(target, tgt_h, "target cloud", v1);  // 左视口显示目标点云
    viewer->addPointCloud(target, tgt_h, "target cloud1", v2); // 右视口显示目标点云
    viewer->addPointCloud(icp, transe, "pcs cloud", v2);       // 右视口显示配准后的点云

    // 启动可视化窗口
    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);  // 每100ms刷新一次视图
        boost::this_thread::sleep(boost::posix_time::microseconds(10000));
    }
}

2.2完整代码

cpp 复制代码

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/registration/icp.h>  // 点到面的ICP算法
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>

// 计算点云法线并将法线信息拼接到点云数据中
void cloud_with_normal(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointNormal>::Ptr& cloud_normals)
{
    pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
    n.setNumberOfThreads(10);  // 设置并行线程数
    n.setInputCloud(cloud);
    n.setSearchMethod(tree);
    n.setKSearch(10);  // 设置近邻点的数量
    n.compute(*normals);  // 计算法线

    // 将点云数据与法线信息拼接
    pcl::concatenateFields(*cloud, *normals, *cloud_normals);
}

// 点到面的ICP配准算法封装
void perform_icp_registration(pcl::PointCloud<pcl::PointNormal>::Ptr& source_with_normals,
    pcl::PointCloud<pcl::PointNormal>::Ptr& target_with_normals, pcl::PointCloud<pcl::PointNormal>::Ptr& p_icp_cloud, Eigen::Matrix4f& transformation)
{
    pcl::IterativeClosestPointWithNormals<pcl::PointNormal, pcl::PointNormal> p_icp;
    p_icp.setInputSource(source_with_normals);
    p_icp.setInputTarget(target_with_normals);
    p_icp.setTransformationEpsilon(1e-10);   // 设置转换终止条件
    p_icp.setMaxCorrespondenceDistance(10);  // 设置最大对应距离
    p_icp.setEuclideanFitnessEpsilon(0.001); // 设置收敛条件
    p_icp.setMaximumIterations(35);          // 设置最大迭代次数
    p_icp.align(*p_icp_cloud);

    std::cout << "\nICP has converged, score is " << p_icp.getFitnessScore() << std::endl;
    transformation = p_icp.getFinalTransformation();
    std::cout << "变换矩阵：\n" << transformation << std::endl;
}

// 可视化原始点云与配准后的点云
void  visualize_registration(pcl::PointCloud<pcl::PointXYZ>::Ptr& source,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& target, pcl::PointCloud<pcl::PointXYZ>::Ptr& icp)
{
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("配准结果"));
    int v1 = 0, v2 = 1;
    viewer->createViewPort(0, 0, 0.5, 1, v1);  // 创建左视口
    viewer->createViewPort(0.5, 0, 1, 1, v2);  // 创建右视口
    viewer->setBackgroundColor(0, 0, 0, v1);   // 设置背景颜色
    viewer->setBackgroundColor(0.05, 0, 0, v2);

    // 设置点云的颜色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(source, 255, 0, 0);  // 源点云颜色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(target, 0, 0, 255);  // 目标点云颜色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transe(icp, 255, 0, 0);    // 配准后的点云颜色
    
    viewer->addPointCloud(source, src_h, "source cloud", v1);  // 添加源点云
    viewer->addPointCloud(target, tgt_h, "target cloud", v1);  // 添加目标点云
    viewer->addPointCloud(target, tgt_h, "target cloud1", v2); // 目标点云
    viewer->addPointCloud(icp, transe, "pcs cloud", v2);       // 添加配准后的点云

    // 启动可视化窗口
    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);  
        boost::this_thread::sleep(boost::posix_time::microseconds(10000));
    }
}

int main()
{
    // 加载源点云数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile<pcl::PointXYZ>("1.pcd", *source);

    // 加载目标点云数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile<pcl::PointXYZ>("2.pcd", *target);

    // 计算法线并拼接到源点云和目标点云
    pcl::PointCloud<pcl::PointNormal>::Ptr source_with_normals(new pcl::PointCloud<pcl::PointNormal>);
    cloud_with_normal(source, source_with_normals);
    pcl::PointCloud<pcl::PointNormal>::Ptr target_with_normals(new pcl::PointCloud<pcl::PointNormal>);
    cloud_with_normal(target, target_with_normals);

    // 点到面的ICP配准
    pcl::PointCloud<pcl::PointNormal>::Ptr p_icp_cloud(new pcl::PointCloud<pcl::PointNormal>);
    Eigen::Matrix4f transformation;
    perform_icp_registration(source_with_normals, target_with_normals, p_icp_cloud, transformation);

    // 使用ICP计算的刚体变换对源点云进行变换
    pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::transformPointCloud(*source, *out_cloud, transformation);

    // 保存配准结果
    pcl::io::savePCDFileASCII("test.pcd", *out_cloud);

    // 可视化配准结果
    visualize_registration(source, target, out_cloud);

    return (0);
}

PCL 点云配准 点到面ICP算法（精配准）

一、概述

1.1原理

1.2实现步骤

1.3应用场景

二、代码实现

2.1关键函数

2.1.1 法线估计和拼接函数

2.1.2 ICP配准算法

2.1.3 点云可视化函数

2.2完整代码

三、实现效果

PCL 点云配准点到面ICP算法（精配准）