在一个项目中需要在回调函数中进行KD树的半径搜索,然后挑选出半径范围内距离最近的点。根据下面的搜索函数,邻域内各点到搜索点的距离存储在一个vector
中。
kdtreePose->radiusSearch(curr_pose, kdtreeSearchDis, pointSearchInd, pointSearchSqDis);
如果距离不是按顺序排列,那么要求出最近点,需要对vector
进行遍历,会增加耗时,因此探究一下距离是否按顺序排列。
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
// 创建点云
cloud->width = 10000;
cloud->height = 1;
cloud->is_dense = false;
cloud->points.resize(cloud->width* cloud->height);
for (size_t i = 0; i < cloud->points.size(); ++i)
{
// 坐标范围在[0,1024),正方体内随机取点
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));
}
// 定义邻域点索引及距离
vector<int> pointSearchInd;
vector<float> pointSearchSqDis;
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
kdtree.setInputCloud(cloud);
// 半径搜索
kdtree.radiusSearch(cloud->points[0], 100, pointSearchInd, pointSearchSqDis, 0);
// 输出结果查看
for (auto& dis : pointSearchSqDis)
{
cout << dis << endl;
}
可以看出,KD树搜索结果是按距离从小到达排列,若要求最近点,可以直接取pointSearchInd[0]
即可。