1. 首页
  2. 文章资讯

PCL 点云配准-K4PCS算法（粗配准）

9 阅读 0 评论

目录

一、概述

1.1原理

1.2实现步骤

1.3应用场景

二、代码实现

2.1关键函数

2.1.1 加载点云数据

2.1.2 执行K4PCS配准

2.1.3 可视化结果

2.2完整代码

三、实现效果

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

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

一、概述

        在3D点云处理中，粗配准算法通常用于初步对齐源点云和目标点云。K-4PCS（4点一致性采样）算法是一种常见的粗配准方法，其主要通过匹配源点云和目标点云中的四个点来求解最佳刚体变换。本次的PCL实现中，通过K4PCS算法实现两个点云的粗配准，并在最终通过可视化进行结果展示。

1.1原理

K-4PCS算法通过以下步骤进行点云配准：

  1. 随机采样源点云中的四个点。
  2. 在目标点云中找到匹配的四个点。
  3. 计算刚体变换（旋转和平移矩阵），使得源点云与目标点云的这四个点对齐。
  4. 重复以上步骤，并选择使匹配点最多的变换作为最终结果。

1.2实现步骤

  1. 加载点云数据：读取源点云和目标点云数据。
  2. K4PCS配准：使用PCL中的 KFPCSInitialAlignment 类，设置相关参数后进行配准。
  3. 点云变换：应用计算得到的刚体变换矩阵，将源点云进行配准。
  4. 结果可视化：通过两个视口展示配准前后的点云结果。

1.3应用场景

  1. 3D扫描数据的初步对齐：在3D扫描数据处理中，K-4PCS算法用于对多个视角的点云数据进行初步对齐，为后续的精确配准（如ICP）提供初始位姿。
  2. 机器人定位与导航：K-4PCS算法可以用于多视角3D点云的匹配，帮助机器人在复杂环境中进行粗配准与定位。

二、代码实现

2.1关键函数

2.1.1 加载点云数据

// 加载点云数据，传入文件路径，返回加载后的点云
pcl::PointCloud<pcl::PointXYZ>::Ptr loadPointCloud(const std::string& file_path) {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>(file_path, *cloud) == -1) {
        PCL_ERROR("Could not read file\n");
        return nullptr;
    }
    return cloud;
}

2.1.2 执行K4PCS配准

// 通过K-4PCS算法进行点云的粗配准，返回配准后的点云
pcl::PointCloud<pcl::PointXYZ>::Ptr runK4PCSAlignment(pcl::PointCloud<pcl::PointXYZ>::Ptr source,
                                                      pcl::PointCloud<pcl::PointXYZ>::Ptr target) {
    pcl::registration::KFPCSInitialAlignment<pcl::PointXYZ, pcl::PointXYZ> kfpcs;
    kfpcs.setInputSource(source);       // 设置源点云
    kfpcs.setInputTarget(target);       // 设置目标点云
    kfpcs.setApproxOverlap(0.7);        // 近似重叠率
    kfpcs.setLambda(0.5);               // 平移矩阵的加权系数
    kfpcs.setDelta(0.002, false);       // 源和目标之间的距离
    kfpcs.setNumberOfThreads(6);        // 多线程加速
    kfpcs.setNumberOfSamples(200);      // 采样点数量

    pcl::PointCloud<pcl::PointXYZ>::Ptr kpcs(new pcl::PointCloud<pcl::PointXYZ>);
    kfpcs.align(*kpcs);                 // 执行配准，返回配准后的点云

    return kpcs;
}

2.1.3 可视化结果

// 可视化点云，展示配准前后的结果
void visualizePointClouds(pcl::PointCloud<pcl::PointXYZ>::Ptr source,
                          pcl::PointCloud<pcl::PointXYZ>::Ptr target,
                          pcl::PointCloud<pcl::PointXYZ>::Ptr aligned) {
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Point Cloud Viewer"));

    int v1(0), v2(1);
    // 第一个视口，显示配准前的点云
    viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
    viewer->setBackgroundColor(0, 0, 0, v1);
    viewer->addText("Before Registration", 10, 10, "v1_text", v1);

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

    // 将源点云设为绿色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_color(source, 0, 255, 0);
    viewer->addPointCloud(source, source_color, "source cloud", v1);

    // 第二个视口，显示配准后的点云
    viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);
    viewer->setBackgroundColor(0.1, 0.1, 0.1, v2);
    viewer->addText("After Registration", 10, 10, "v2_text", v2);

    // 配准后的点云设为红色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> aligned_color(aligned, 255, 0, 0);
    viewer->addPointCloud(aligned, aligned_color, "aligned cloud", v2);

    // 开始渲染
    viewer->spin();
}

2.2完整代码

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/console/time.h>   
#include <pcl/registration/ia_kfpcs.h> // K4PCS算法头文件
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>

// 加载点云数据，传入文件路径，返回加载后的点云
pcl::PointCloud<pcl::PointXYZ>::Ptr loadPointCloud(const std::string& file_path) {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>(file_path, *cloud) == -1) {
        PCL_ERROR("Could not read file\n");
        return nullptr;
    }
    return cloud;
}

// 通过K-4PCS算法进行点云的粗配准，返回配准后的点云
pcl::PointCloud<pcl::PointXYZ>::Ptr runK4PCSAlignment(pcl::PointCloud<pcl::PointXYZ>::Ptr source,
    pcl::PointCloud<pcl::PointXYZ>::Ptr target) {
    pcl::registration::KFPCSInitialAlignment<pcl::PointXYZ, pcl::PointXYZ> kfpcs;
    kfpcs.setInputSource(source);       // 设置源点云
    kfpcs.setInputTarget(target);       // 设置目标点云
    kfpcs.setApproxOverlap(0.7);        // 近似重叠率
    kfpcs.setLambda(0.5);               // 平移矩阵的加权系数
    kfpcs.setDelta(0.002, false);       // 源和目标之间的距离
    kfpcs.setNumberOfThreads(6);        // 多线程加速
    kfpcs.setNumberOfSamples(200);      // 采样点数量

    pcl::PointCloud<pcl::PointXYZ>::Ptr kpcs(new pcl::PointCloud<pcl::PointXYZ>);
    kfpcs.align(*kpcs);                 // 执行配准，返回配准后的点云

    return kpcs;
}

// 可视化点云，展示配准前后的结果
//void visualizePointClouds(pcl::PointCloud<pcl::PointXYZ>::Ptr source,
//    pcl::PointCloud<pcl::PointXYZ>::Ptr target,
//    pcl::PointCloud<pcl::PointXYZ>::Ptr aligned) {
//    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Point Cloud Viewer"));
//
//    int v1(0), v2(1);
//    // 第一个视口，显示配准前的点云
//    viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
//    viewer->setBackgroundColor(0, 0, 0, v1);
//    viewer->addText("Before Registration", 10, 10, "v1_text", v1);
//
//    // 将目标点云设为蓝色
//    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target, 0, 0, 255);
//    viewer->addPointCloud(target, target_color, "target cloud", v1);
//
//    // 将源点云设为绿色
//    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_color(source, 0, 255, 0);
//    viewer->addPointCloud(source, source_color, "source cloud", v1);
//
//    // 第二个视口，显示配准后的点云
//    viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);
//    viewer->setBackgroundColor(0.1, 0.1, 0.1, v2);
//    viewer->addText("After Registration", 10, 10, "v2_text", v2);
//
//    // 配准后的点云设为红色
//    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> aligned_color(aligned, 255, 0, 0);
//    viewer->addPointCloud(aligned, aligned_color, "aligned cloud", v2);
//    //viewer->addPointCloud(target, target_color, "target cloud2", v2);
//    viewer->addPointCloud(source, source_color, "source cloud2", v2);
//
//    // 开始渲染
//    viewer->spin();
//}

void visualizePointClouds(pcl::PointCloud<pcl::PointXYZ>::Ptr source,
    pcl::PointCloud<pcl::PointXYZ>::Ptr target,
    pcl::PointCloud<pcl::PointXYZ>::Ptr aligned) {
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Point Cloud Viewer"));

    int v1(0), v2(1);
    // 第一个视口，显示配准前的点云
    viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
    viewer->setBackgroundColor(0, 0, 0, v1);
    viewer->addText("Before Registration", 10, 10, "v1_text", v1);

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

    // 将源点云设为绿色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_color(source, 0, 255, 0);
    viewer->addPointCloud(source, source_color, "source cloud", v1);

    // 第二个视口，显示配准后的点云和目标点云
    viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);
    viewer->setBackgroundColor(0.1, 0.1, 0.1, v2);
    viewer->addText("After Registration", 10, 10, "v2_text", v2);

    // 配准后的点云设为红色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> aligned_color(aligned, 255, 0, 0);
    viewer->addPointCloud(aligned, aligned_color, "aligned cloud", v2);

    // 将目标点云也显示在配准后的视口中，设为蓝色
    viewer->addPointCloud(target, target_color, "target cloud 2", v2);

    // 开始渲染
    viewer->spin();
}

int main(int argc, char** argv) {
    // 加载源点云和目标点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr source = loadPointCloud("1.pcd");
    pcl::PointCloud<pcl::PointXYZ>::Ptr target = loadPointCloud("2.pcd");

    // 执行K4PCS配准
    pcl::PointCloud<pcl::PointXYZ>::Ptr aligned = runK4PCSAlignment(source, target);

    // 可视化点云结果
    visualizePointClouds(source, target, aligned);

    return 0;
}

三、实现效果

本站资源均来自互联网，仅供研究学习，禁止违法使用和商用，产生法律纠纷本站概不负责！如果侵犯了您的权益请与我们联系！

转载请注明出处： 免费源码网-免费的源码资源网站 » PCL 点云配准-K4PCS算法（粗配准）

点赞(0) 打赏

评论列表 共有 0 条评论

暂无评论

微信公众账号

微信扫一扫加关注

 发表
评论 返回
顶部