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;
}