PCL用KDtree,给搜索到的邻近点上色

用KDtree,给搜索到的邻近点上色。

复制代码
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include <pcl/search/kdtree.h> // 包含kdtree头文件
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/common/centroid.h>

typedef pcl::PointXYZ PointT;

int main()
{
	pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
	pcl::io::loadPCDFile("../new.pcd", *cloud);

	pcl::visualization::PCLVisualizer viewer; 
	viewer.setBackgroundColor(100, 100, 100); // rgb
	pcl::visualization::PointCloudColorHandlerCustom<PointT> red1(cloud, 0, 0, 0); // rgb
	viewer.addPointCloud(cloud, red1, "cloud");
    //先画一个底图



	// 定义KDTree对象
	pcl::search::KdTree<PointT>::Ptr kdtree(new pcl::search::KdTree<PointT>);
	kdtree->setInputCloud(cloud); // 设置要搜索的点云,建立KDTree

	std::vector<int> indices; // 存储查询近邻点索引
	std::vector<float> distances; // 存储近邻点对应距离的平方

	PointT point = cloud->points[0]; // 初始化一个查询点
	
	// 查询距point最近的k个点
	int k = 1000;
	int size = kdtree->nearestKSearch(point, k, indices, distances);

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudOut(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::copyPointCloud(*cloud, indices, *cloudOut);
	pcl::visualization::PointCloudColorHandlerCustom<PointT> red(cloudOut, 255, 0, 0); // rgb
	viewer.addPointCloud(cloudOut, red, "cloudOut");
    //按照索引单独给点云上色上色并叠加到点云上

	// 查询point半径为radius邻域球内的点
	std::vector<int> indices1; // 存储查询近邻点索引
	std::vector<float> distances1; // 存储近邻点对应距离的平方
	double radius = 30.0;
	int size1 = kdtree->radiusSearch(point, radius, indices1, distances1);

	std::cout << "search point : " << size1 << std::endl;

	viewer.spin();

	// 2. 非阻塞式
	while (!viewer.wasStopped())
	{
		viewer.spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
		// 可添加其他操作
	}

	system("pause");
    
	return 0;
}
相关推荐
jz_ddk9 分钟前
[实战]调频(FM)和调幅(AM)信号生成(完整C语言实现)
c语言·算法·信号处理
CloudAce云一25 分钟前
谷歌云代理商:谷歌云TPU/GPU如何加速您的AI模型训练和推理
算法
轻语呢喃1 小时前
每日LeetCode : 杨辉三角
javascript·后端·算法
YuTaoShao2 小时前
【LeetCode 热题 100】148. 排序链表——(解法二)分治
java·算法·leetcode·链表
Shilong Wang2 小时前
三维旋转沿轴分解
算法·计算机视觉·机器人
ygming2 小时前
Q43- code973- 最接近原点的 K 个点 + Q44- code347- 前 K 个高频元素
前端·算法
lightqjx2 小时前
【数据结构】顺序表(sequential list)
c语言·开发语言·数据结构·算法
ygming2 小时前
Hashmap/ Hashset- Q39~Q42内容
前端·算法
蒟蒻小袁3 小时前
力扣面试150题--全排列
算法·leetcode·面试
mit6.8243 小时前
[Backlog] 核心协调器 | 终端用户界面(TUI)实现 | 多分支任务冲突解决 | 测试验证体系
人工智能·算法