(八)k-dTree库教程二--k-d树在PCL中的应用

k-d树在PCL中的应用

上一讲中,我们介绍了k-d树在二维上的原理,k-d树构建算法和k-d树是如何应用于最近邻查找算法的。对于PCL中的k-d树来说,只不过是将二维提升到了三维而已。下面我们就看看PCL是如何将k-d树应用于范围搜索和最近邻搜索的。

PCL中使用的k-d树的算法来自FLANN(Fast Library for Approximate Nearest Neighbors),它是目前最完整的(近似)最近邻开源库。不但实现了一系列查找算法,还包含了一种自动选取最快算法的机制。

K近邻搜索

K近邻搜索是给定查询点及正整数K,从数据集中找到距离查询点最近的K个数据。

头文件

#include <pcl/kdtree/kdtree_flann.h>

步骤

  1. 初始化k-d树
// 初始化kdTree
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
// 设置要搜索的点云
kdtree.setInputCloud(cloud);
  1. 输入查询点和正整数K进行搜索
kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance)

关键函数:

int 
      nearestKSearch (const PointT &point, int k, 
                      std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const;
  • 输入输出参数:
    • point[in]:查询点
    • k[in]:正整数K,即要搜索的最近邻点的数目
    • k_indices[out]:搜索到的最近邻点在点云中的下标
    • k_sqr_distances[out]:搜索到的最近邻点距离查询点的距离的平方值
    • 返回值:搜索到的最近邻点的数目

范围搜索

范围搜索就是给定查询点和查询距离的阈值,从数据集中找出所有与查询点距离小于阈值的数据。

头文件

#include <pcl/kdtree/kdtree_flann.h>

步骤

  1. 初始化k-d树
// 初始化kdTree
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
// 设置要搜索的点云
kdtree.setInputCloud(cloud);
  1. 输入查询点和搜索半径进行搜索
kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance)

关键函数:

int 
      radiusSearch (const PointT &point, double radius, std::vector<int> &k_indices,
                    std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const;
  • 输入输出参数:
    • point[in]:查询点
    • radius[in]:搜索半径
    • k_indices[out]:搜索到的最近邻点在点云中的下标
    • k_sqr_distances[out]:搜索到的最近邻点距离查询点的距离的平方值
    • max_nn[in]:默认为0,搜索到的点的上限,如果搜索到的点数目多于它,那么丢弃超出的部分;如果max_nn设为0或者大于搜索点云的大小,则返回所有找到的点。
    • 返回值:搜索到的最近邻点的数目

程序

/*
 * 功能:kdTree的应用,包括k最近邻搜索和范围搜索
*/


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

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

using namespace std;

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;
    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);          // c++中rand()函数生成的范围:0~RAND_MAX,
        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.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近邻搜索

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

    // 以radius为半径的范围搜索

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

    system("pause");
    return 0;
}
  • 2
    点赞
  • 6
    收藏
    觉得还不错? 一键收藏
  • 1
    评论

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

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

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值