目录

一、概述

1.1原理

1.2实现步骤

1.3应用场景

二、代码实现

2.1关键函数

2.1.1 可视化函数

2.1.2 最远点采样

2.2完整代码

三、实现效果

---

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

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

---

一、概述

        最远点采样（FPS） 是一种常用的点云降采样方法，旨在从原始点云中选择一组具有代表性的点，以保持点云的几何特征。该方法通过每次选择距离已选定点最远的点，从而实现均匀分布的采样。

1.1原理

        最远点采样的基本思想是：在每次迭代中，从未选中的点中选择一个与已选点集中距离最远的点。通过这种方式，可以确保所选点在空间上的分布尽可能均匀。这种方法特别适用于处理大规模点云数据

1.2实现步骤

1. 读取点云数据。
2. 初始化选定点集合，设置采样数量。
3. 在未选中的点中找到距离已选点最远的点并添加到选定点集合中。
4. 可视化原始点云和采样后的点云。

1.3应用场景

1. 点云简化：在保留几何特征的情况下减少点云的数量。
2. 特征提取：从点云中提取具有代表性的特征点。
3. 实时处理：在需要快速处理的场景中使用降采样的点云。

二、代码实现

2.1关键函数

2.1.1 可视化函数

通过设置不同的视口和背景颜色，分别显示原始点云和采样后的点云。

// 可视化原始点云和采样后的点云
void visualizePointClouds(
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,  // 原始点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr sampled_cloud)  // 采样后的点云
{
    // 创建可视化器
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("ShowCloud"));

    // 创建视口1，显示原始点云
    int vp_1;
    viewer->createViewPort(0.0, 0.0, 0.5, 1.0, vp_1);  // 创建左侧窗口
    viewer->setBackgroundColor(1.0, 1.0, 1.0, vp_1);  // 设置白色背景
    viewer->addText("Raw Point Clouds", 10, 10, "v1_text", vp_1);  // 添加标题

    // 创建视口2，显示采样后的点云
    int vp_2;
    viewer->createViewPort(0.5, 0.0, 1.0, 1.0, vp_2);  // 创建右侧窗口
    viewer->setBackgroundColor(0.98, 0.98, 0.98, vp_2);  // 设置浅灰色背景
    viewer->addText("FPS Point Clouds", 10, 10, "v2_text", vp_2);  // 添加标题

    // 添加点云到视口
    viewer->addPointCloud<pcl::PointXYZ>(cloud, "sample cloud", vp_1);  // 添加原始点云
    viewer->addPointCloud<pcl::PointXYZ>(sampled_cloud, "cloud_filtered", vp_2);  // 添加采样后的点云
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, "sample cloud", vp_1);  // 设置颜色为红色
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0, "cloud_filtered", vp_2);  // 设置颜色为绿色

    // 启动可视化循环
    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);  // 刷新可视化器
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));  // 睡眠以避免CPU占用过高
    }
}

2.1.2 最远点采样

使用 PCL 提供的 FarthestPointSampling 类进行最远点采样，并设置相关参数。

注：该函数为PCL1.13.0新增

// 最远点采样
pcl::PointCloud<pcl::PointXYZ>::Ptr farthestPointSampling(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, int sample_size)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr sampled_cloud(new pcl::PointCloud<pcl::PointXYZ>);  // 存储采样后的点云
    std::unordered_set<int> sampled_indices;  // 用于存储已选择点的索引

    // 随机选择第一个点
    int first_index = rand() % cloud->points.size();
    sampled_indices.insert(first_index);  // 添加到已选择集合
    sampled_cloud->points.push_back(cloud->points[first_index]);  // 添加到采样点云

    // 进行采样，直到达到指定数量
    while (sampled_cloud->points.size() < sample_size)
    {
        std::vector<float> distances(cloud->points.size(), std::numeric_limits<float>::max());  // 初始化距离向量
        for (const auto& index : sampled_indices)
        {
            for (size_t i = 0; i < cloud->points.size(); ++i)
            {
                if (sampled_indices.find(i) == sampled_indices.end())  // 确保点没有被选择
                {
                    float distance = euclideanDistance(cloud->points[index], cloud->points[i]); // 计算距离
                    distances[i] = std::min(distances[i], distance);  // 更新最小距离
                }
            }
        }

        // 找到距离最远的点
        int farthest_index = std::distance(distances.begin(), std::max_element(distances.begin(), distances.end()));
        sampled_indices.insert(farthest_index);  // 将该点添加到已选择点集
        sampled_cloud->points.push_back(cloud->points[farthest_index]);  // 添加到采样点云
    }
    
    sampled_cloud->width = sampled_cloud->points.size();  // 更新宽度
    sampled_cloud->height = 1;  // 设置高度
    sampled_cloud->is_dense = true;  // 确保点云是密集的

    return sampled_cloud;  // 返回采样后的点云
}

2.2完整代码

// C++头文件
#include <cmath>
#include <random>
// PCL头文件
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/filters/farthest_point_sampling.h> // 最远点采样
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>

// 可视化原始点云和采样后的点云
void visualizePointClouds(
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,  // 原始点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr sampled_cloud)  // 采样后的点云
{
    // 创建可视化器
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("cloud"));

    // 创建视口1，显示原始点云
    int vp_1;
    viewer->createViewPort(0.0, 0.0, 0.5, 1.0, vp_1);  // 创建左侧窗口
    viewer->setBackgroundColor(0, 0, 0, vp_1);  // 设置黑色背景
    viewer->addText("Raw point clouds", 10, 10, "v1_text", vp_1);  // 添加标题

    // 创建视口2，显示采样后的点云
    int vp_2;
    viewer->createViewPort(0.5, 0.0, 1.0, 1.0, vp_2);  // 创建右侧窗口
    viewer->setBackgroundColor(0.1, 0.1, 0.1, vp_2);  // 设置灰色背景
    viewer->addText("FPS point clouds", 10, 10, "v2_text", vp_2);  // 添加标题

    // 添加点云到视口
    viewer->addPointCloud<pcl::PointXYZ>(cloud, "sample cloud", vp_1);
    viewer->addPointCloud<pcl::PointXYZ>(sampled_cloud, "cloud_filtered", vp_2);
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, "sample cloud", vp_1);  // 设置颜色为红色
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0, "cloud_filtered", vp_2);  // 设置颜色为绿色

    // 启动可视化循环
    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);  // 刷新可视化器
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));  // 睡眠以避免CPU占用过高
    }
}

int main(int argc, char** argv)
{
    // ------------------------------读取点云数据---------------------------------
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile("dragon.pcd", *cloud) < 0)
    {
        PCL_ERROR("Could not read file\n");
        return (-1);  // 返回错误
    }
    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZ>);

    // -------------------------------最远点采样---------------------------------
    std::random_device rd;
    int random_seed = rd();   // 随机选取一个点作为种子点
    int sampleNum = 10000;    // 采样点个数
    pcl::FarthestPointSampling<pcl::PointXYZ> fps;
    fps.setInputCloud(cloud); // 读取点云
    fps.setSeed(random_seed); // 设置第一个种子点
    fps.setSample(sampleNum); // 设置采样点个数
    fps.filter(*filtered);    // 进行采样


    // ------------------------------结果可视化----------------------------------
    visualizePointClouds(cloud, filtered);  // 调用可视化函数

    return (0);
}

三、实现效果