【三维点云数据处理】Kd-Tree实现最近邻搜索

文章目录

目录

文章目录

前言

一、kd-tree是什么？

二、代码实现

1.关键函数

2.代码示例

三、结果展示
​​​​​​​

---

前言

点云数据处理中最为核心的问题就是建立离散点间的拓扑关系 , 实现基于邻域关系的快速查找。

---

一、kd-tree是什么？

        kd-tree 或者 k 维树是计算机科学中使用的一种数据结构 ， 用来组织表示 k 维空间中点集合。它是一种带有其他约束条件的二分查找树。kd-tree 对于区间和近邻搜索十分有用。我们为了达到目的 , 通常只在三个维度中进行处理 , 因此所有的 kd-tree 都将是三维 kd-tree。如图 所示 ，kd-tree 的每一级在指定维度上分开所有的子节点。在树的根部所有子节点是以第一个指定的维度上被分开 ( 也就是说 , 如果第一维坐标小于根节点的点它将分在左边的子树中，如果大于根节点的点它将分在右边的子树中 )。树的每一级都在下一个维度上分开 ，所有其他的维度用完之后就回到第一个维度。建立 kd-tree 最高效的方法是，像快速分类一样使用分割法 , 把指定维度的值放在根上，在该维度上包含较小数值的在左子树，较大的在右子树。然后分别在左边和右边的子树上重复这个过程 , 直到用户准备分类的最后一个树仅仅由一个元素组成。

创建k-d树

有很多种方法可以选择轴垂直分割面（ axis-aligned splitting planes ），所以有很多种创建k-d树的方法。 最典型的方法如下：

• 随着树的深度轮流选择轴当作分割面。（例如：在三维空间中根节点是 x 轴垂直分割面，其子节点皆为 y 轴垂直分割面，其孙节点皆为 z 轴垂直分割面，其曾孙节点则皆为 x 轴垂直分割面，依此类推。）

• 点由垂直分割面之轴座标的中位数区分并放入子树

这个方法产生一个平衡的k-d树。每个叶节点的高度都十分接近。然而，平衡的树不一定对每个应用都是最佳的。 [3] 

最邻近搜索

最邻近搜索用来找出在树中与输入点最接近的点。

k-d树最邻近搜索的过程如下：

1. 从根节点开始，递归的往下移。往左还是往右的决定方法与插入元素的方法一样(如果输入点在分区面的左边则进入左子节点，在右边则进入右子节点)。

2. 一旦移动到叶节点，将该节点当作"当前最佳点"。

3. 解开递归，并对每个经过的节点运行下列步骤：

   1. 如果当前所在点比当前最佳点更靠近输入点，则将其变为当前最佳点。

   2. 检查另一边子树有没有更近的点，如果有则从该节点往下找。

4. 当根节点搜索完毕后完成最邻近搜索。

二、代码实现

1.关键函数

virtual int nearestKSearch (const PointT &p_q, int k, std: :vector<< int >&
k_indices, std; ;vector<float >&k_sqr_distances) const =0

纯虚函数，具体实现在其子类 KdTresFLANN 中，其用来进行 K 邻域搜索，参数 p_q 为需要查询的点，k 为 K 邻域个数，k_indices 为搜索完的邻域点对应的索引，k_sqr_distances 为搜索完的每个邻域点与查询点之间的欧氏阶离。 

2.代码示例

#include <QCoreApplication>
#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
            << " " << searchPoint.z
            << ") with K=" << K << std::endl;

  if ( kdtree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0 )
  {
    for (size_t i = 0; i < pointIdxNKNSearch.size (); ++i)
      std::cout << "    "  <<   cloud->points[ pointIdxNKNSearch[i] ].x
                << " " << cloud->points[ pointIdxNKNSearch[i] ].y
                << " " << cloud->points[ pointIdxNKNSearch[i] ].z
                << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
  }

  // Neighbors within radius search

  std::vector<int> pointIdxRadiusSearch;
  std::vector<float> pointRadiusSquaredDistance;

  float radius = 256.0f * rand () / (RAND_MAX + 1.0f);

  std::cout << "Neighbors within radius search at (" << searchPoint.x
            << " " << searchPoint.y
            << " " << searchPoint.z
            << ") with radius=" << radius << std::endl;


  if ( kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 )
  {
    for (size_t i = 0; i < pointIdxRadiusSearch.size (); ++i)
      std::cout << "    "  <<   cloud->points[ pointIdxRadiusSearch[i] ].x
                << " " << cloud->points[ pointIdxRadiusSearch[i] ].y
                << " " << cloud->points[ pointIdxRadiusSearch[i] ].z
                << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
  }


  return 0;
}

---

三、结果展示