PCL库学习笔记——KdTree查找

最新推荐文章于 2025-06-19 16:05:54 发布

王勇21633012

最新推荐文章于 2025-06-19 16:05:54 发布

阅读量8.7k

点赞数 5

CC 4.0 BY-SA版权

分类专栏：计算机视觉 PCL 点云库立体视觉视觉SLAM

本文链接：https://blog.youkuaiyun.com/weixin_38275649/article/details/80972554

本篇主要演示使用KdTree查找特定点、位置的K近邻或最近邻，以及用户指定半径范围内查找所有近邻，并计算出距离。

KdTree是计算机科学中用来组织K维数据点集的数据结构。类似二叉树。在三位点云中主要用的是三维的KdTree，这种数据结构能提供快速的查找近邻算法。

代码：

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

  // 创建随机点云
  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);
  }

  // 创建一个 KdTree 对象
  pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;

  // 将前面创建的随机点云作为 KdTree 输入
  kdtree.setInputCloud (cloud);

  // 创建一个随机的点作为查找中心
  pcl::PointXYZ searchPoint;

  searchPoint.x = 1024.0f * rand () / (RAND_MAX