PCL 最远点采样(FPS)
目录
一、概述
1.1原理
1.2实现步骤
1.3应用场景
二、代码实现
2.1关键函数
2.1.1 可视化函数
2.1.2 最远点采样
2.2完整代码
三、实现效果
PCL点云算法汇总及实战案例汇总的目录地址链接：
PCL点云算法与项目实战案例汇总（长期更新）
一、概述
最远点采样（FPS） 是一种常用的点云降采样方法，旨在从原始点云中选择一组具有代表性的点，以保持点云的几何特征。该方法通过每次选择距离已选定点最远的点，从而实现均匀分布的采样。
1.1原理
最远点采样的基本思想是：在每次迭代中，从未选中的点中选择一个与已选点集中距离最远的点。通过这种方式，可以确保所选点在空间上的分布尽可能均匀。这种方法特别适用于处理大规模点云数据
1.2实现步骤
- 读取点云数据。
- 初始化选定点集合，设置采样数量。
- 在未选中的点中找到距离已选点最远的点并添加到选定点集合中。
- 可视化原始点云和采样后的点云。
1.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);
}
三、实现效果
