3d激光雷达开发（kd树）

Posted 2022-03-08 费晓行

【 声明：版权所有，欢迎转载，请勿用于商业用途。 联系信箱：feixiaoxing @163.com】

        kd 树是点云数据的一种管理方式。如果打个比方，就像一串数据一样，可以用数组串起来，也可以用二叉树串起来。用特定的数据结构把数据管理起来，最终还是为了更快更好的查找数据。比如如果我们查找一个点周围有哪些数据，此时没有kd树这样的数据结构的话，就只能一个一个去遍历了，效率非常低。参考的代码网页在这里，https://pcl.readthedocs.io/projects/tutorials/en/master/kdtree_search.html#kdtree-search

1、编写kdtree_search.cp

#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>

#include <iostream>
#include <vector>
#include <ctime>

int
main ()

  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 (std::size_t i = 0; i < cloud->size (); ++i)
  
    (*cloud)[i].x = 1024.0f * rand () / (RAND_MAX + 1.0f);
    (*cloud)[i].y = 1024.0f * rand () / (RAND_MAX + 1.0f);
    (*cloud)[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> pointIdxKNNSearch(K);
  std::vector<float> pointKNNSquaredDistance(K);

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

  if ( kdtree.nearestKSearch (searchPoint, K, pointIdxKNNSearch, pointKNNSquaredDistance) > 0 )
  
    for (std::size_t i = 0; i < pointIdxKNNSearch.size (); ++i)
      std::cout << "    "  <<   (*cloud)[ pointIdxKNNSearch[i] ].x 
                << " " << (*cloud)[ pointIdxKNNSearch[i] ].y 
                << " " << (*cloud)[ pointIdxKNNSearch[i] ].z 
                << " (squared distance: " << pointKNNSquaredDistance[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 (std::size_t i = 0; i < pointIdxRadiusSearch.size (); ++i)
      std::cout << "    "  <<   (*cloud)[ pointIdxRadiusSearch[i] ].x 
                << " " << (*cloud)[ pointIdxRadiusSearch[i] ].y 
                << " " << (*cloud)[ pointIdxRadiusSearch[i] ].z 
                << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
  


  return 0;

2、代码讲解

        整个代码主要分成三个部分。第一，创建一个KdTree的变量；第二，随机生成一个点，查找周围最近的10个数据；第三，给一个半径数据，查找半径之内的所有数据。

3，准备CMakeLists.txt

 cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(kdtree_search)
 
 find_package(PCL 1.2 REQUIRED)
 
 include_directories($PCL_INCLUDE_DIRS)
 link_directories($PCL_LIBRARY_DIRS)
 add_definitions($PCL_DEFINITIONS)

add_executable (kdtree_search kdtree_search.cpp)
target_link_libraries (kdtree_search $PCL_LIBRARIES)

4、生成sln文件，直接编译

        按照常规，mkdir build & cd build & cmake ..。

        编译sln之后，不出意外，应该可以看到这样的界面，

5、直接运行kdtree_search.exe

        直接运行，可以会失败，因为缺少pcl_kdtree_debug.dll库，补上即可。

        打开一个终端，输入kdtree_search.exe，应该就可以看到这样的打印信息，

        数据内容可能不太一样，但是形式肯定是这样的。