在本教程中,我们将详细介绍如何使用KdTree来查找特定点或位置的K个最近邻居,然后我们将继续介绍如何在用户指定的半径范围内找到所有邻居(在本例中为随机) 。
#理论引入
kd树或k维树是计算机科学中用于在具有k维的空间中组织若干点的数据结构。这是一个二叉搜索树,其他约束条件是强加给它的。Kd树对于范围和最近的邻居搜索非常有用。就我们的目的而言,我们通常只会在三维空间中处理点云,所以我们所有的kd树都是三维的。kd树的每个级别都使用与相应轴垂直的超平面沿特定维度分割所有的孩子。在树的根部,所有的孩子都将根据第一维进行分割(即,如果第一维坐标小于根,它将在左子树中,并且如果它大于根,则显然将在正确的子树)。树下的每一层在下一个维度上分开,一旦所有其他维度都用尽,则返回到第一维。构建kd树的最有效的方法是使用像Quick Sort一样使用的分区方法将中间点放在根上,并将所有具有较小一维值的东西放在左边,并且放大到右边。然后,在左侧和右侧的子树上重复此过程,直到要分区的最后一棵树仅由一个元素组成。
来自Wikipedia:
一个二维kd树的例子
这是一个二维kd树的例子
这是最近邻居搜索工作的一个小时的示范。
#代码
用你最喜欢的编辑器创建一个名为kdtree_search.cpp
的文件,并在里面放置以下内容:
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <iostream>
#include <vector>
#include <ctime>
int
main (int argc, char** argv)
{
srand (time (NULL));
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
// Generate pointcloud data
cloud->width = 1000;
cloud->height = 1;
cloud->points.resize (cloud->width * cloud->height);
for (size_t i = 0; i < cloud->points.size (); ++i)
{
cloud->points[i].x = 1024.0f * rand () / (RAND_MAX + 1.0f);
cloud->points[i].y = 1024.0f * rand () / (RAND_MAX + 1.0f);
cloud->points[i].z = 1024.0f * rand () / (RAND_MAX + 1.0f);
}
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
kdtree.setInputCloud (cloud);
pcl::PointXYZ searchPoint;
searchPoint.x = 1024.0f * rand () / (RAND_MAX + 1.0f);
searchPoint.y = 1024.0f * rand () / (RAND_MAX + 1.0f);
searchPoint.z = 1024.0f * rand () / (RAND_MAX + 1.0f);
// K nearest neighbor search
int K = 10;
std::vector<int> pointIdxNKNSearch(K);
std::vector<float> pointNKNSquaredDistance(K);
std::cout << "K nearest neighbor search at (" << searchPoint.x
<< " " << searchPoint.y
<< "