目录
1.半径搜索
Kd-tree搜索方式有两种基本的方式:一种是范围查询(range searches),另一种是K近邻查询(K-neighbor searches)。 K近邻查询是给定查询点及正整数K,从数据集中找到距离查询点最近的K个数据,当K=1时,就是最近邻查询(nearest neighbor searches)。范围查询就是给定查询点和查询距离的阈值,从数据集中找出所有与查询点距离小于阈值的数据;
2.代码
#include <iostream>
#include <vector>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h> //kdtree
#include <pcl/visualization/pcl_visualizer.h>//可视化
#include <boost/thread/thread.hpp>
using namespace std;
int main()
{
//读取点云数据
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("bunny.pcd", *cloud) == -1)
{
PCL_ERROR("Cloudn't load pointCloud file!");
return -1;
}
//建立kd-tree
pcl::KdTreeFLANN<pcl::PointXYZRGB> kdtree; //建立kdtree对象
kdtree.setInputCloud(cloud); //设置输入的点云
//K近邻搜索
pcl::PointXYZRGB searchPoint = cloud->points[0]; //设置查找点
cloud->points[0].r = 0;//查询点着色(蓝色)
cloud->points[0].g = 0;
cloud->points[0].b = 255;
float radius = 0.01; //设置需要查找半径
vector<int> pointIdxRadiusSearch; //近邻点的索引
vector<float> pointRadiusSquaredDistance; //保存每个近邻点与查找点之间的欧式距离平方
if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
{
cout << pointIdxRadiusSearch.size() << endl;
for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i) {
cloud->points[pointIdxRadiusSearch[i]].r = 255;//近邻点着色为粉红色
cloud->points[pointIdxRadiusSearch[i]].g = 0;
cloud->points[pointIdxRadiusSearch[i]].b = 255;
}
}
//可视化
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
viewer->addPointCloud<pcl::PointXYZRGB>(cloud, "cloud");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud"); // 设置点云大小
while (!viewer->wasStopped())
{
viewer->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(10000));
}
return 0;
}