PCL 点云配准 KD-ICP算法(精配准)

发布于:2024-10-17 ⋅ 阅读:(42) ⋅ 点赞:(0)

目录

一、概述

1.1原理

1.2实现步骤

1.3应用场景

二、代码实现

2.1关键函数

2.1.1 加载点云函数

2.1.2 构建KD树函数

2.1.3 KD-ICP配准函数

2.1.4 点云可视化函数

2.2完整代码

三、实现效果


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

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


一、概述

        KD-ICP(基于KD树的ICP)算法 是 ICP(Iterative Closest Point)算法 的一种改进形式,主要通过 KD树(K-Dimensional Tree) 加速最近邻搜索,显著提高了ICP算法的配准效率。KD树的使用使得ICP在处理大规模点云数据时具备更高的性能,因为KD树能够在多维空间中快速找到最近邻点。相比于传统ICP,KD-ICP更适用于实时3D点云处理以及大型点云数据的配准。

1.1原理

        ICP算法通过迭代最近邻点配对来计算两个点云之间的刚体变换。KD-ICP使用KD树加速最近邻搜索,主要流程如下:

  1. 最近邻搜索:使用KD树结构快速查找源点云和目标点云中的最近邻点对。
  2. 刚体变换计算:通过最小化源点云和目标点云最近邻点之间的误差,计算出最优的刚体变换矩阵。
  3. 应用刚体变换:将该变换应用于源点云并更新其位置。
  4. 终止条件:当收敛条件满足时,停止迭代。

1.2实现步骤

  1. 加载源点云和目标点云。
  2. 构建KD树:为源点云和目标点云构建KD树结构,加速最近邻搜索。
  3. 初始化ICP算法:设置ICP的最大迭代次数、距离阈值、转换误差等参数。
  4. 执行KD-ICP配准:通过KD树进行最近邻搜索,计算刚体变换,并更新源点云的位置。
  5. 可视化:展示源点云、目标点云及配准后的源点云

1.3应用场景

  1. 3D物体扫描与拼接:在3D扫描重建过程中,将多个不同角度获取的点云通过KD-ICP配准拼接成一个完整模型。
  2. 机器人视觉:机器人视觉中通过KD-ICP对环境点云数据进行对齐,实现导航和物体定位。
  3. 自动驾驶:在自动驾驶中,KD-ICP可用于车辆环境感知的多传感器数据融合,例如激光雷达点云数据的实时配准。

二、代码实现

2.1关键函数

2.1.1 加载点云函数

该函数用于从PCD文件中加载点云数据,源点云和目标点云都会通过此函数读取。

pcl::PointCloud<pcl::PointXYZ>::Ptr loadPointCloud(const std::string& filename) {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
    if (pcl::io::loadPCDFile<pcl::PointXYZ>(filename, *cloud) == -1) {
        PCL_ERROR("无法读取点云文件 %s\n", filename.c_str());
        return nullptr;
    }
    std::cout << "从文件 " << filename << " 读取点云,包含 " << cloud->size() << " 个点\n";
    return cloud;
}

2.1.2 构建KD树函数

KD树加速最近邻搜索,分别为源点云和目标点云构建KD树

pcl::search::KdTree<pcl::PointXYZ>::Ptr buildKDTree(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
    tree->setInputCloud(cloud);
    return tree;
}

2.1.3 KD-ICP配准函数

该函数用于执行基于KD树的ICP算法,实现精确的点云配准。

Eigen::Matrix4f performKDICP(pcl::PointCloud<pcl::PointXYZ>::Ptr source, pcl::PointCloud<pcl::PointXYZ>::Ptr target, 
                             pcl::search::KdTree<pcl::PointXYZ>::Ptr tree1, pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2) {
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    icp.setSearchMethodSource(tree1);
    icp.setSearchMethodTarget(tree2);
    icp.setInputSource(source);
    icp.setInputTarget(target);
    icp.setMaxCorrespondenceDistance(1);        // 设置对应点之间的最大距离
    icp.setMaximumIterations(35);               // 设置最大迭代次数
    icp.setTransformationEpsilon(1e-10);        // 设置收敛条件下的最小变换差异
    icp.setEuclideanFitnessEpsilon(0.05);       // 设置收敛的均方误差阈值

    pcl::PointCloud<pcl::PointXYZ>::Ptr icp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    icp.align(*icp_cloud);
    
    if (icp.hasConverged()) {
        std::cout << "ICP 收敛,得分为 " << icp.getFitnessScore() << std::endl;
        std::cout << "变换矩阵:\n" << icp.getFinalTransformation() << std::endl;
    } else {
        std::cout << "ICP 未能收敛\n";
    }

    return icp.getFinalTransformation();
}

2.1.4 点云可视化函数

该函数用于可视化配准前后的点云,配准后的点云显示为绿色,目标点云显示为红色。

void visualizePointClouds(pcl::PointCloud<pcl::PointXYZ>::Ptr source, pcl::PointCloud<pcl::PointXYZ>::Ptr target,
                          pcl::PointCloud<pcl::PointXYZ>::Ptr icp_cloud) {
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("KD-ICP 配准结果"));

    viewer->setBackgroundColor(0, 0, 0);
    
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target, 255, 0, 0);
    viewer->addPointCloud(target, target_color, "target cloud");
    
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> icp_color(icp_cloud, 0, 255, 0);
    viewer->addPointCloud(icp_cloud, icp_color, "icp cloud");

    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "icp cloud");

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

2.2完整代码

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>         // 引入ICP配准算法
#include <pcl/search/kdtree.h>            // 引入KD树加速最近邻搜索
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/console/time.h>             // 用于计算配准时间

// 加载点云数据函数
// 该函数用于从PCD文件中加载点云数据,如果文件加载失败会返回nullptr
pcl::PointCloud<pcl::PointXYZ>::Ptr loadPointCloud(const std::string& filename) {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
    if (pcl::io::loadPCDFile<pcl::PointXYZ>(filename, *cloud) == -1) {
        PCL_ERROR("无法读取点云文件 %s\n", filename.c_str());
        return nullptr;
    }
    std::cout << "从文件 " << filename << " 读取点云,包含 " << cloud->size() << " 个点\n";
    return cloud;
}

// 构建KD树函数
// 为了加速最近邻搜索,该函数为输入的点云构建一个KD树
pcl::search::KdTree<pcl::PointXYZ>::Ptr buildKDTree(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
    tree->setInputCloud(cloud);
    return tree;
}

// KD-ICP配准函数
// 该函数执行基于KD树的ICP配准,通过设置ICP参数和构建的KD树,进行点云配准并返回最终的变换矩阵
Eigen::Matrix4f performKDICP(pcl::PointCloud<pcl::PointXYZ>::Ptr source, pcl::PointCloud<pcl::PointXYZ>::Ptr target, 
                             pcl::search::KdTree<pcl::PointXYZ>::Ptr tree1, pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2) {
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;

    // 设置KD树用于加速最近邻搜索
    icp.setSearchMethodSource(tree1);
    icp.setSearchMethodTarget(tree2);

    // 设置ICP输入点云,源点云与目标点云
    icp.setInputSource(source);
    icp.setInputTarget(target);

    // 设置ICP的参数
    icp.setMaxCorrespondenceDistance(1);       // 设置对应点对之间的最大距离
    icp.setMaximumIterations(35);              // 设置最大迭代次数
    icp.setTransformationEpsilon(1e-10);       // 为终止条件设置最小转换差异
    icp.setEuclideanFitnessEpsilon(0.05);      // 设置收敛条件:当均方误差和小于该阈值时停止迭代

    // 存储配准结果的点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr icp_cloud(new pcl::PointCloud<pcl::PointXYZ>);

    // 执行ICP配准
    icp.align(*icp_cloud);

    // 判断ICP是否收敛并输出结果
    if (icp.hasConverged()) {
        std::cout << "ICP 收敛,得分为 " << icp.getFitnessScore() << std::endl;
        std::cout << "变换矩阵:\n" << icp.getFinalTransformation() << std::endl;
    } else {
        std::cout << "ICP 未能收敛\n";
    }

    // 返回最终的刚体变换矩阵
    return icp.getFinalTransformation();
}

// 点云可视化函数
// 该函数用于可视化原始点云(源点云与目标点云)及配准后的点云
void visualizePointClouds(pcl::PointCloud<pcl::PointXYZ>::Ptr source, pcl::PointCloud<pcl::PointXYZ>::Ptr target,
                          pcl::PointCloud<pcl::PointXYZ>::Ptr icp_cloud) {
    // 创建PCL可视化对象
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("KD-ICP 配准结果"));

    // 设置背景颜色为黑色
    viewer->setBackgroundColor(0, 0, 0);

    // 将目标点云上色为红色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target, 255, 0, 0);
    viewer->addPointCloud(target, target_color, "target cloud");

    // 将配准后的源点云上色为绿色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> icp_color(icp_cloud, 0, 255, 0);
    viewer->addPointCloud(icp_cloud, icp_color, "icp cloud");

    // 设置点云的显示属性(点大小为1)
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "icp cloud");

    // 运行可视化窗口,直到关闭窗口
    while (!viewer->wasStopped()) {
        viewer->spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }
}

int main(int argc, char** argv) {
    pcl::console::TicToc time;   // 用于计算执行时间

    // 加载源点云和目标点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr source = loadPointCloud("1.pcd");
    pcl::PointCloud<pcl::PointXYZ>::Ptr target = loadPointCloud("2.pcd");

    // 构建KD树
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree1 = buildKDTree(source);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2 = buildKDTree(target);

    // 使用KD树加速的ICP配准
    time.tic();  // 开始计时
    Eigen::Matrix4f final_transform = performKDICP(source, target, tree1, tree2);
    std::cout << "配准时间: " << time.toc() << " ms" << std::endl;  // 输出配准时间

    // 配准后将源点云进行变换
    pcl::PointCloud<pcl::PointXYZ>::Ptr icp_cloud(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::transformPointCloud(*source, *icp_cloud, final_transform);

    // 可视化原始点云与配准后的点云
    visualizePointClouds(source, target, icp_cloud);

    return 0;
}

三、实现效果

ICP 收敛,得分为 8.32129e-06
变换矩阵:
  0.914549   -0.38993   0.107491   0.023871
  0.345635    0.89144   0.293038 -0.0615766
 -0.210087  -0.230845   0.950039  0.0535896
         0          0          0          1


网站公告

今日签到

点亮在社区的每一天
去签到