PCL：kd-tree

最新推荐文章于 2023-02-25 17:17:45 发布

原创最新推荐文章于 2023-02-25 17:17:45 发布 · 1.1k 阅读

1 ·

CC 4.0 BY-SA版权

文章标签：

#kd-tree #pcl #C++

SLAM 专栏收录该内容

39 篇文章

订阅专栏

其实还蛮早就听过kd-tree的，但是对其中的原理一直是一知半解，之前听Ng讲ML的时候，说要对kd-tree，O(),等一些基本概念要有所了解，当时也没有人带，不知道具体的学习一些机器学习包括深度学习的一些流程，上来就是卷积神经网络等等，理解不算深刻，到现在才稍稍有点明白其中一些关系。

何为kd-tree？都是外国人发明的简称，全名是k dimensional search tree，说白了，就是一个维度为k的二叉树。在算法导论的二叉树那章可以看看，具体就明白了，kd-tree 也是就一种数据结构，因为LZ主要是处理三维视觉方面的问题，所以接触的都是三维的空间点云，所以建立的kd-tree都是三维的，也就不拓展到n维了，或许后面可以加上时间流，变成四维的，也就是处理点云视频？

怎么构建树呢？在树的根部，所有的孩子都将根据第一维度（维度选择是不一定的，这里以第一维度为例）进行分割（即，如果第一维坐标小于根，它将在左子树中，如果大于根，则显然会在右子树）。树中的每个级别在下一维度上划分，一旦所有其他人都已用完，则返回到第一个维度。构建k-d树最有效的方法是使用像“快速排序”一样使用的分区方法，将中点放在根部，而一切的值都小于左边的一维值，而右边的值越大。然后，可以在左右子树上重复此过程，直到您要分区的最后一个树只由一个元素组成。

这里写图片描述

#include <iostream>
#include <vector>
#include <ctime>
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <boost/concept_check.hpp>


int main(int argc, char **argv) {
    srand(time(NULL));
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

    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);


//       kNN
      int K = 10;
      std::vector<int> pointIdxNKNSearch(K);
      std::vector<float> pointNKNSquareDistance(K);

      std::cout << "K nearest neighbour search at ( " << searchPoint.x
             <<  " " << searchPoint.y 
             <<  " " << searchPoint.z << " ) with K = " << K << std::endl;

      if(kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquareDistance) > 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: " << pointNKNSquareDistance[i] << " )" << std::endl;
      }

//       neighbours within radius search
      std::vector<int> pointIdxRadiusSearch;
      std::vector<float> pointRadiusSquaredDistance;

      float radius = 256.0f * rand() / (RAND_MAX + 1.0f);
      std::cout << "Neighbours within radius search at ( "<< searchPoint.x
        <<  " " << searchPoint.y 
        <<  " " << searchPoint.z << " ) with radius = " << radius  << std::endl;

      std::cout << " if is " << kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) << std::endl;
      if(kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)

      {
    std::cout << "the size of pointIdxRadiusSearch is  " << pointIdxRadiusSearch.size() << std::endl;
    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;
}