#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <vector>
#include <ctime>
using namespace std;
int main()
{
srand(time(NULL));
//随机生成一个点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cloud->width = 1000;
cloud->height = 1;
cloud->is_dense = false;
cloud->points.resize(cloud->width * cloud->height);
for (size_t i = 0; i < cloud->points.size(); i++)
{
cloud->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
cloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
cloud->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);
}
//定义kdtree,相当于一个容器
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
kdtree.setInputCloud(cloud);
//随机生成一个中心点
pcl::PointXYZ keypoint;
keypoint.x = 1024 * rand() / (RAND_MAX + 1.0f);
keypoint.y = 1024 * rand() / (RAND_MAX + 1.0f);
keypoint.z = 1024 * rand() / (RAND_MAX + 1.0f);
//K近邻搜索,用到两个vector,一个存下标,一个存距离中心点的长度
vector<int> KNsearch_idx;
vector<float> KNsearch_dis;
int K = 10;
cout << "the K_nearestSearch at" << keypoint.x << " " << keypoint.y << " " << keypoint.z << endl;
if (kdtree.nearestKSearch(keypoint, K, KNsearch_idx, KNsearch_dis) > 0)
{
for (size_t i = 0; i < KNsearch_idx.size(); i++)
{
cout << cloud->points[KNsearch_idx[i]].x << " " << cloud->points[KNsearch_idx[i]].y << " " << cloud->points[KNsearch_idx[i]].z << endl;
cout << KNsearch_dis[i] << endl;
}
}
//搜索半径R范围内的所有近邻
vector<int> Rsearch_idx;
vector<float> Rsearch_dis;
int R = 256 * rand() / (RAND_MAX + 1.0f);
cout << "In " << R << " field at " << keypoint.x << " " << keypoint.y << " " << keypoint.z << endl;
if (kdtree.radiusSearch(keypoint, R, Rsearch_idx, Rsearch_dis) > 0)
{
for (size_t i = 0; i < Rsearch_idx.size(); i++)
{
cout << cloud->points[Rsearch_idx[i]].x << " " << cloud->points[Rsearch_idx[i]].y << " " << cloud->points[Rsearch_idx[i]].z << endl;
cout << Rsearch_dis[i] << endl;
}
}
return 0;
}
自己选择的路,跪着也要走完。朋友们,虽然这个世界日益浮躁起来,只要能够为了当时纯粹的梦想和感动坚持努力下去,不管其它人怎么样,我们也能够保持自己的本色走下去。