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;
}
相关推荐
岁忧14 分钟前
(LeetCode 面试经典 150 题 ) 11. 盛最多水的容器 (贪心+双指针)
java·c++·算法·leetcode·面试·go
chao_78917 分钟前
二分查找篇——搜索旋转排序数组【LeetCode】两次二分查找
开发语言·数据结构·python·算法·leetcode
秋说2 小时前
【PTA数据结构 | C语言版】一元多项式求导
c语言·数据结构·算法
Maybyy2 小时前
力扣61.旋转链表
算法·leetcode·链表
卡卡卡卡罗特4 小时前
每日mysql
数据结构·算法
chao_7895 小时前
二分查找篇——搜索旋转排序数组【LeetCode】一次二分查找
数据结构·python·算法·leetcode·二分查找
lifallen6 小时前
Paimon 原子提交实现
java·大数据·数据结构·数据库·后端·算法
lixzest6 小时前
C++ Lambda 表达式详解
服务器·开发语言·c++·算法
EndingCoder6 小时前
搜索算法在前端的实践
前端·算法·性能优化·状态模式·搜索算法
丶小鱼丶6 小时前
链表算法之【合并两个有序链表】
java·算法·链表