PCL的KDTree距离搜索结果排列顺序

在一个项目中需要在回调函数中进行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]即可。