IterativeClosestPoint＜PointXYZL, PointXYZL, double＞类是 PCL (Point Cloud Library) 中用于执行迭代最近点（ICP）算法的

IterativeClosestPoint 类是 PCL (Point Cloud Library) 中用于执行迭代最近点（ICP）算法的类。这个算法用于计算两个点云之间的最佳对齐（即找到一个刚体变换，使得一个点云尽可能接近另一个点云）。以下是这个类的一些常用函数和它们的用法：

1. setInputSource

   • 用于设置源点云（要对齐的点云）。
   • 用法：icp.setInputSource(source_cloud); 其中 source_cloud 是一个 PointCloud::Ptr 类型的指针。

2. setInputTarget

   • 用于设置目标点云（源点云将会尝试对齐到这个点云）。
   • 用法：icp.setInputTarget(target_cloud); 其中 target_cloud 是一个 PointCloud::Ptr 类型的指针。

3. align

   • 执行 ICP 算法，尝试将源点云对齐到目标点云。
   • 用法：icp.align(aligned_cloud); 其中 aligned_cloud 是一个 PointCloud 类型的对象，用于存储对齐后的源点云。

4. getFinalTransformation

   • 获取从源点云到目标点云的最终变换矩阵。
   • 用法：Eigen::Matrix4d transformation = icp.getFinalTransformation();

5. setMaxCorrespondenceDistance

   • 设置源点云和目标点云之间的最大对应点距离。如果两个点之间的距离大于这个值，它们将不会被视为对应点。
   • 用法：icp.setMaxCorrespondenceDistance(distance); 其中 distance 是一个 double 类型的值。

6. setMaximumIterations

   • 设置 ICP 算法的最大迭代次数。
   • 用法：icp.setMaximumIterations(iterations); 其中 iterations 是一个 int 类型的值。

7. setTransformationEpsilon

   • 设置算法的变换矩阵收敛阈值。当变换矩阵的改变量小于这个阈值时，算法会停止迭代。
   • 用法：icp.setTransformationEpsilon(epsilon); 其中 epsilon 是一个 double 类型的值。

8. setEuclideanFitnessEpsilon

   • 设置收敛条件，即当算法的平均误差小于这个阈值时停止迭代。
   • 用法：icp.setEuclideanFitnessEpsilon(epsilon); 其中 epsilon 是一个 double 类型的值。

使用这些函数，你可以配置 ICP 算法的各种参数，执行点云对齐，并获取结果。这在计算机视觉和机器人学中的许多应用中非常有用，如三维重建、SLAM 和物体识别。

匹配效果

代码

#include 
#include 
#include 
#include 
#include 

int main() {
    // 创建点云对象
    pcl::PointCloud<pcl::PointXYZL>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZL>());
    pcl::PointCloud<pcl::PointXYZL>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZL>());

    // 生成源点云数据
    source_cloud->width = 500;
    source_cloud->height = 1;
    source_cloud->points.resize(source_cloud->width * source_cloud->height);
    for (auto& point : *source_cloud) {
        point.x = 1024 * rand() / (RAND_MAX + 1.0f);
        point.y = 1024 * rand() / (RAND_MAX + 1.0f);
        point.z = 1024 * rand() / (RAND_MAX + 1.0f);
        point.label = 1;
    }

    // // 生成目标点云数据（对源点云进行简单平移）
    // *target_cloud = *source_cloud;
    // for (auto& point : *target_cloud) {
    //     point.x += 0.5f;
    //     point.y += 0.5f;
    //     point.z += 0.5f;
    // }

    *target_cloud = *source_cloud;
    for (auto& point : *target_cloud)
    {
        point.x += 0.1f;  // 减小平移量
        point.y += 0.1f;  // 减小平移量
        point.z += 0.1f;  // 减小平移量
    }

    // // 创建 IterativeClosestPoint 对象并设置参数
    // pcl::IterativeClosestPoint icp;
    // icp.setInputSource(source_cloud);
    // icp.setInputTarget(target_cloud);
    // icp.setMaxCorrespondenceDistance(0.05);  // 可根据需要调整
    // icp.setMaximumIterations(50);            // 可根据需要调整
    // icp.setTransformationEpsilon(1e-8);      // 可根据需要调整
    // icp.setEuclideanFitnessEpsilon(1);       // 可根据需要调整

    // 创建 IterativeClosestPoint 对象并设置参数
    pcl::IterativeClosestPoint<pcl::PointXYZL, pcl::PointXYZL, double> icp;
    icp.setInputSource(source_cloud);
    icp.setInputTarget(target_cloud);
    icp.setMaxCorrespondenceDistance(0.1);  // 增大对应点距离
    icp.setMaximumIterations(100);          // 增加迭代次数
    icp.setTransformationEpsilon(1e-10);    // 减小变换矩阵收敛阈值
    icp.setEuclideanFitnessEpsilon(1e-5);   // 减小平均误差收敛阈值


    // 对齐点云
    pcl::PointCloud<pcl::PointXYZL>::Ptr aligned_cloud(new pcl::PointCloud<pcl::PointXYZL>());
    icp.align(*aligned_cloud);

    if (!icp.hasConverged()) {
        PCL_ERROR("ICP did not converge.\n");
        return -1;
    }
// 创建可视化对象
    pcl::visualization::PCLVisualizer viewer("Point Clouds");

    // 创建两个视口
    int v1(0);
    int v2(1);
    viewer.createViewPort(0.0, 0.0, 0.5, 1.0, v1);
    viewer.createViewPort(0.5, 0.0, 1.0, 1.0, v2);

    // 设置颜色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZL> source_color(source_cloud, 255, 0, 0); // 红色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZL> target_color(target_cloud, 0, 255, 0); // 绿色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZL> aligned_color(aligned_cloud, 0, 0, 255); // 蓝色

    // 添加点云到视口
    viewer.addPointCloud<pcl::PointXYZL>(source_cloud, source_color, "source cloud", v1);
    viewer.addPointCloud<pcl::PointXYZL>(target_cloud, target_color, "target cloud", v1);
    viewer.addPointCloud<pcl::PointXYZL>(aligned_cloud, aligned_color, "aligned cloud", v2);
    viewer.addPointCloud<pcl::PointXYZL>(target_cloud, target_color, "target cloud v2", v2);

    // 设置背景颜色和窗口属性
    viewer.setBackgroundColor(0, 0, 0, v1);
    viewer.setBackgroundColor(0.05, 0.05, 0.05, v2);

    // 显示窗口直到用户关闭它
    while (!viewer.wasStopped()) {
        viewer.spinOnce();
    }

    return 0;

    return 0;
}

编译文件

cmake_minimum_required(VERSION 3.0 FATAL_ERROR)
project(icp_visualization)

# 寻找 PCL 库
find_package(PCL 1.8 REQUIRED)

# 包含 PCL 头文件
include_directories(${PCL_INCLUDE_DIRS})

# 链接 PCL 库
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

# 创建可执行文件
add_executable(icp_visualization icp_visualization.cpp)

# 链接 PCL 库到你的可执行文件
target_link_libraries(icp_visualization ${PCL_LIBRARIES})

代码解读

这段 C++ 代码使用 PCL (Point Cloud Library) 生成了两个随机点云，应用了迭代最近点（ICP）算法来对齐这两个点云，并在一个窗口的两个视口中显示原始的未对齐点云和对齐后的点云。以下是代码的逐步解释：

1. 创建点云对象:

   • 初始化两个 pcl::PointCloud 类型的智能指针，source_cloud 和 target_cloud，用于存储源点云和目标点云。

2. 生成源点云数据:

   • 填充 source_cloud 为具有 500 个随机生成的点的点云。每个点的 XYZ 坐标在 0 到 1024 之间随机分布，每个点的标签设为 1。

3. 生成目标点云数据:

   • 将 source_cloud 复制到 target_cloud，然后对 target_cloud 中的每个点进行小幅平移（每个坐标轴增加 0.1 单位）。这创建了一个与源点云相似但略有偏移的目标点云。

4. 设置 ICP 算法的参数:

   • 创建 pcl::IterativeClosestPoint 类的实例 icp，用于对齐源点云到目标点云。
   • 设置源点云（source_cloud）和目标点云（target_cloud）。
   • 调整 ICP 参数，如最大对应点距离、最大迭代次数、变换矩阵收敛阈值和平均误差收敛阈值，以提高对齐的精度和性能。

5. 对齐点云:

   • 执行 ICP 算法，并将对齐结果存储在 aligned_cloud 中。
   • 检查 ICP 算法是否成功收敛。

6. 可视化:

   • 创建一个 pcl::visualization::PCLVisualizer 可视化对象 viewer。
   • 在 viewer 中创建两个视口 v1 和 v2。
   • 设置源点云为红色、目标点云为绿色、对齐后的点云为蓝色，并将它们添加到相应的视口中。
   • 在两个视口中分别显示原始未对齐的点云和对齐后的点云。
   • 运行一个循环，以便用户可以查看点云直到窗口被关闭。