利用KDTree近邻搜索

原理的相关介绍:KD-Tree详解: 从原理到编程实现

 k-d树(k-dimensional树的简称),是一种分割k维数据空间的数据结构。主要应用于多维空间关键数据的搜索(如:范围搜索和最近邻搜索)。K-D树是二进制空间分割树的特殊的情况。

索引结构中相似性查询有两种基本的方式:一种是范围查询(range searches),另一种是K近邻查询(K-neighbor searches)。范围查询就是给定查询点和查询距离的阈值,从数据集中找出所有与查询点距离小于阈值的数据;K近邻查询是给定查询点及正整数K,从数据集中找到距离查询点最近的K个数据,当K=1时,就是最近邻查询(nearest neighbor searches)。

PCL中类pcl::KdTree<PointT>是kd-tree数据结构的实现。并且提供基于FLANN进行快速搜索的一些相关子类与包装类。具体可以参考相应的API。下面给出2个类的具体用法。

  • pcl::search::KdTree < PointT >
    pcl::search::KdTree<PointT>pcl::search::Search< PointT >的子类,是pcl::KdTree<PointT>的包装类。包含(1) k 近邻搜索;(2) 邻域半径搜索。
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/search/kdtree.h> // 包含kdtree头文件

typedef pcl::PointXYZ PointT;

int main()
{
	pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
	pcl::io::loadPCDFile("../input_cloud/20230605_132512.pcd", *cloud);

	// 定义KDTree对象
	pcl::search::KdTree<PointT>::Ptr kdtree(new pcl::search::KdTree<PointT>);
	kdtree->setInputCloud(cloud); // 设置要搜索的点云,建立KDTree

	std::vector<int> indices; // 存储查询近邻点索引
	std::vector<float> distances; // 存储近邻点对应距离的平方

	PointT point = cloud->points[0]; // 初始化一个查询点

	// 查询距point最近的k个点
	int k = 10;
	int size = kdtree->nearestKSearch(point, k, indices, distances);

	std::cout << "nearest search point : " << size << std::endl;

	// 查询point半径为radius邻域球内的点
	double radius = 2.0;
	size = kdtree->radiusSearch(point, radius, indices, distances);

	std::cout << "radius search point : " << size << std::endl;

	system("pause");
	return 0;
}

 注意: 搜索结果默认是按照距离point点的距离从近到远排序;如果InputCloud中含有point点,搜索结果的的第一个点是point本身。

 

  • pcl::KdTreeFLANN < PointT >
    pcl::KdTreeFLANN<PointT>pcl::KdTree<PointT>的子类,可以实现同样的功能。
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
// 包含相关头文件
#include <pcl/kdtree/kdtree_flann.h>

typedef pcl::PointXYZ PointT;

int main()
{
	pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
	pcl::io::loadPCDFile("../input_cloud/20230605_132833.pcd", *cloud);

	pcl::KdTreeFLANN<pcl::PointXYZ> kdtree; //创建KDtree
	kdtree.setInputCloud(cloud); // 设置要搜索的点云,建立KDTree

	std::vector<int> indices; // 存储查询近邻点索引
	std::vector<float> distances; // 存储近邻点对应距离的平方

	PointT point = cloud->points[1]; // 初始化一个查询点

	// 查询距point最近的k个点
	int k = 10;
	int size = kdtree.nearestKSearch(point, k, indices, distances);

	std::cout << "nearest search point : " << size << std::endl;

	// 查询point半径为radius邻域球内的点
	double radius = 2.0;
	size = kdtree.radiusSearch(point, radius, indices, distances);

	std::cout << "radius search point : " << size << std::endl;

	system("pause");
	return 0;
}


srand(time(NULL));

        srand(time(NULL));是一个常用的用于设置随机数种子的代码片段。它使用当前时间作为种子,以便每次程序运行时生成不同的随机数序列。
        要使用srand()函数生成伪随机数,需要包含头文件<cstdlib><ctime>

pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
pcl::search::KdTree<pcl::PointXYZ> kdtree;

这两行代码都是使用了 PCL(Point Cloud Library)中的 KdTree 数据结构,但有一些细微的区别:

  1. 类型声明:
            第一行代码使用了 pcl::KdTreeFLANN<pcl::PointXYZ> 类型,该类型实现了基于FLANN库的 KdTree,用于点云的最近邻搜索等操作。
            第二行代码使用了 pcl::search::KdTree<pcl::PointXYZ> 类型,该类型是 PCL 中的另一个 KdTree 实现,用于点云的最近邻搜索和半径搜索等操作。

  2. 实现差异:
           pcl::KdTreeFLANN 是基于 FLANN(Fast Library for Approximate Nearest Neighbors)库实现的 KdTree。FLANN 库提供了高效的最近邻搜索算法,支持多种数据类型和距离度量方式,并且对大规模数据集具有良好的扩展性。
           pcl::search::KdTree 是在 PCL 中自定义的 KdTree 实现,它也支持最近邻搜索和半径搜索等操作,但可能不如 FLANN 库提供的实现在某些情况下高效。

        因此,选择使用哪个 KdTree 类型取决于你的需求以及对性能和功能的要求。如果你需要更高效和灵活的最近邻搜索算法,可以选择第一行代码中的 pcl::KdTreeFLANN 类型;如果你对性能要求不高,或更偏向于使用 PCL 默认的实现,可以选择第二行代码中的 pcl::search::KdTree 类型。


if (kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0) 

在Kd树(K-dimensional tree)中,nearestKSearch()函数是用于搜索K个最近邻的函数。它可以帮助我们找到给定查询点附近最接近的K个点。

nearestKSearch()函数通常具有以下参数:

  1. query_point: 要查询的点,它是一个K维向量或数组。
  2. k: 要搜索的最近邻数量,也就是要返回的最接近的K个点。
  3. result: 用于存储搜索结果的容器,通常是一个空的列表或数组。
  4. distances: 用于存储每个最近邻点与查询点之间的距离的容器,通常也是一个空的列表或数组。

size_t i = 0;

         size_t是C++中的一种无符号整数类型,被用于表示内存大小、数组长度等非负整数值。它的大小在不同的编译器和操作系统下可能会有所不同,但通常被设计为足够大以容纳目标平台上最大可能的对象大小。
        在C++中,size_t类型通常被用作循环计数器、数组索引和指针算术等场景中。它可以保证能够表示当前平台上最大可能的对象大小,并且不受特定平台的位数限制(如32位或64位)的影响。


为什么循环计数时用size_t而不是int?

 在循环计数时,使用size_t类型比使用int类型更好的原因是因为:

  1. 无符号性:size_t是一种无符号整数类型,它只表示非负的整数值。这意味着你不需要担心负数循环计数的问题。当循环计数器必须始终是非负数时,使用size_t能提供更好的语义。

  2. 内存大小:size_t的大小在不同平台上可能会有所变化,但通常情况下,它足够大以容纳较大的数组或容器的元素数量。相比之下,int类型的大小是固定的,在大多数平台上,int类型的大小是4个字节(32位),其范围通常为-2,147,483,648到2,147,483,647(即-2^31到2^31-1)。并且可能无法容纳大型数据结构的元素数量。

  3. 兼容性:某些库或函数接口中使用了size_t类型来表示长度、索引或容量。如果你在循环计数中使用int而不是size_t,可能会导致类型不匹配的错误或警告。

总体而言,使用size_t类型进行循环计数可以提供更好的代码清晰性、可移植性和兼容性,特别是当处理大型数据结构时。然而,在某些情况下,使用int类型也是可以接受的,特别是当循环计数器的范围明确限制在整数范围内时。


viewer.addPointCloud<pcl::PointXYZ>(searchCloud, searchColorHandler, "search_cloud");

在函数viewer.addPointCloud<pcl::PointXYZ>(searchCloud, searchColorHandler, "search_cloud")中,这三个参数的含义如下:

  1. searchCloud: 这是要添加到可视化器中的点云对象。它是一个指向pcl::PointCloud<pcl::PointXYZ>类型的指针。

  2. searchColorHandler: 这是颜色处理器,用于设置点云的颜色。它是pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>类型的对象。

  3. "search_cloud": 这是点云对象的名称。可视化器使用该名称来标识和管理不同的点云。


viewer.addCoordinateSystem(200); 

         viewer.addCoordinateSystem(200);是PCL中的一个函数调用,用于在可视化器中添加一个坐标系。
        这个函数有几个重载版本,参数200表示坐标系的尺寸大小(单位为mm)。它指定了坐标系的轴的长度,即x、y和z轴的长度。

// K nearest neighbor search 搜索K个最近邻居
#include <iostream>
#include <vector>
#include <ctime>

#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/search/kdtree.h>
#include <pcl/search/impl/search.hpp>
#include <pcl/visualization/cloud_viewer.h>


int main(int argc, char** argv) {
    // 用系统时间初始化随机种子
    srand(time(NULL));

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

    // 生成点云数据1000个
    cloud->width = 1000;
    cloud->height = 1;  // 1 表示点云为无序点云
    cloud->points.resize(cloud->width * cloud->height);

    // 给点云填充数据 0 - 1023
    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的实现类KdTreeFLANN (Fast Library for Approximate Nearest Neighbor)
    //pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    pcl::search::KdTree<pcl::PointXYZ> kdtree;
    kdtree.setInputCloud(cloud);  // 设置搜索空间,把cloud作为输入

    pcl::PointCloud<pcl::PointXYZ>::Ptr searchCloud(new pcl::PointCloud<pcl::PointXYZ>);
    //pcl::PointXYZ searchPoint(300, 100, 50);
    
    // 初始化一个随机的点,作为查询点   
    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);
    searchCloud->push_back(searchPoint);

    //创建K和两个向量来保存搜索到的数据
    int K = 10;  //表示搜索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
            << " (距离: " << pointNKNSquaredDistance[i] << ")" << std::endl;
    }

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> originColorHandler(cloud, 255, 255, 255);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> searchColorHandler(searchCloud, 0, 255, 0);

    pcl::visualization::PCLVisualizer viewer("PCL Viewer");
    viewer.setBackgroundColor(0.3, 0.2, 0.5);
    viewer.addPointCloud<pcl::PointXYZ>(cloud, originColorHandler,"cloud");
    viewer.addPointCloud<pcl::PointXYZ>(searchCloud, searchColorHandler, "search_cloud");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "search_cloud");

    pcl::PointXYZ originPoint(0.0, 0.0, 0.0);    
    viewer.addLine(originPoint, searchPoint, "line");  // 添加从原点到搜索点的线段
    viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.9373, 1, 0.3216, "line");    
    viewer.addCoordinateSystem(200); // 指定坐标轴的长度,单位mm
    
    while (!viewer.wasStopped()) {
        viewer.spinOnce();
    }

    return 0;
}


if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)

        在Kd树(K-dimensional tree)中,radiusSearch()函数是用于搜索指定半径内的点的函数。它可以帮助我们找到给定查询点附近指定半径范围内的所有点。

radiusSearch()函数通常具有以下参数:

  1. query_point: 要查询的点,它是一个K维向量或数组。
  2. radius: 搜索的半径范围,只返回距离查询点小于等于半径的点。
  3. result: 用于存储搜索结果的容器,通常是一个空的列表或数组。
  4. distances: 用于存储每个点与查询点之间的距离的容器,通常也是一个空的列表或数组。

        返回值表示在指定半径范围内找到的点的数量。这个返回值是一个整数,表示满足条件的点的数量。 

// Neighbors within radius search 通过指定半径搜索
#include <iostream>
#include <vector>
#include <ctime>

#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/search/kdtree.h>
#include <pcl/search/impl/search.hpp>
#include <pcl/visualization/cloud_viewer.h>


int main(int argc, char** argv) {  
    srand(time(NULL)); // 用系统时间初始化随机种子

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

    // 生成点云数据1000个
    cloud->width = 1000;
    cloud->height = 1;  // 1 表示点云为无序点云
    cloud->points.resize(cloud->width * cloud->height);

    // 给点云填充数据 0 - 1023
    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的实现类KdTreeFLANN (Fast Library for Approximate Nearest Neighbor)
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    //pcl::search::KdTree<pcl::PointXYZ> kdtree;
    kdtree.setInputCloud(cloud);  // 设置搜索空间,把cloud作为输入

    pcl::PointCloud<pcl::PointXYZ>::Ptr searchCloud(new pcl::PointCloud<pcl::PointXYZ>);
    // 初始化一个随机的点,作为查询点   
    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);
    searchCloud->push_back(searchPoint);
    
    std::vector<int> pointIdxRadiusSearch;
    std::vector<float> pointRadiusSquaredDistance;

    // 创建一个随机[0,50)的半径值
    float radius = 50.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
            << " (距离平方:: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
    }

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> originColorHandler(cloud, 255, 255, 255);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> searchColorHandler(searchCloud, 0, 255, 0);

    pcl::visualization::PCLVisualizer viewer("PCL Viewer");
    viewer.setBackgroundColor(0.1176, 0.1176, 0.2353);
    viewer.addPointCloud<pcl::PointXYZ>(cloud, originColorHandler, "cloud");
    viewer.addPointCloud<pcl::PointXYZ>(searchCloud, searchColorHandler, "search_cloud");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "search_cloud");

    pcl::PointXYZ originPoint(0.0, 0.0, 0.0);
    viewer.addLine(originPoint, searchPoint, "line");  // 添加从原点到搜索点的线段
    viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 0.2784, "line");
    viewer.addSphere(searchPoint, radius, 0.945, 0.4118, 0.6941,"sphere",0);  // 添加一个以搜索点为圆心,搜索半径为半径的球体229
    viewer.addCoordinateSystem(200); // 指定坐标轴的长度,单位mm

    while (!viewer.wasStopped()) {
        viewer.spinOnce();
    }

    return 0;
}

 

  • 1
    点赞
  • 14
    收藏
    觉得还不错? 一键收藏
  • 1
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值