ICode9
精准搜索请尝试： 精确搜索
首页
编程语言>
数据库
系统相关
互联网
其他分享
Python
Java
JavaScript
android
PHP
首页 > 其他分享> 文章详细

PCL点云库调库学习系列——k-d tree与八叉树

2021-09-21 09:59:57  阅读：291  来源： 互联网

标签：std distance tree 八叉树 squared points PCL 近邻 cloud


k-d tree与八叉树

1 k-d tree与八叉树

本文并不涉及具体原理的解释，文章着重在k-d树与八叉树在近邻搜索方面的API的使用

1.1 k-d tree

k-d tree算法及原理： https://www.cnblogs.com/flyinggod/p/8727584.html

实现功能

  • 使用k-d tree找到某个具体点的k近邻
  • 在指定半径内搜索其近邻

关键API

//k近邻搜索
int pcl::search::KdTree< PointT, Tree >::nearestKSearch (
    const PointT &point, 				//搜索点
    int k, 								//搜索个数	
    std::vector<int> &k_indices, 		//保存近邻的索引值
    std::vector<float> &k_distances 	//保存近邻的平方距离
)const

//半径radius内近邻搜索
int pcl::search::KdTree< PointT, Tree >::radiusSearch (
    const PointT &point, 				//搜索点
    double radius, 					//搜索半径
    std::vector<int> &k_indices, 		//保存近邻的索引值
    std::vector<float> &k_sqr_dists, 	//保存近邻的平方距离
    unsigned int max_nn				//默认值0,如果给定一个数值，则将返回近邻的最大值限制在这个数值以下(包括该数值)，如果给0或者高于输入点云中的个数，则返回半径中的所有近邻
)const

完整代码

    #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>);
    
        // Generate pointcloud data
        //随机点云生成
        cloud->width = 1000;    //点云数量 
        cloud->height = 1;      //无序点云
        cloud->points.resize(cloud->width * cloud->height);
    
        for (std::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;     //创建KdTreeFLANN对象
    
        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
        /*--------------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;
        
        //核心函数nearestKSearch()，第三个参数的类型为pcl::Indices，进一步参看该类型可以发现为vector<int>，这也就是为什么前面pointIdxNKNSearch的类型声明为vector<int>
        if (kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
        {
            for (std::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;
        }
    
        // 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;
    
        //radiusSearch最后一个参数，用于限制满足近邻条件后的结果的个数，默认值为0，代表全部输出
        if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance, 3) > 0)
        {
            for (std::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;
    }

运行结果

//k近邻搜索结果
K nearest neighbor search at (443.625 766.75 839.094) with K=10
    450.813 818.625 869.688 (squared distance: 3678.65)
    449.688 737.063 784.75 (squared distance: 3871.34)
    445.344 825.344 864.063 (squared distance: 4059.62)
    497.906 737.563 874.625 (squared distance: 5060.83)
    392.219 728.281 870.125 (squared distance: 5085.39)
    437.094 762.938 766.313 (squared distance: 5354.3)
    463.188 814.344 780.063 (squared distance: 6132.54)
    441.531 791.875 745.313 (squared distance: 9430.57)
    465.75 811.781 754.781 (squared distance: 9625.93)
    491.125 677.094 821 (squared distance: 10621.9)

//指定半径内的搜索结果
Neighbors within radius search at (443.625 766.75 839.094) with radius=130.57
    450.813 818.625 869.688 (squared distance: 3678.65)
    449.688 737.063 784.75 (squared distance: 3871.34)
    445.344 825.344 864.063 (squared distance: 4059.62)
    497.906 737.563 874.625 (squared distance: 5060.83)
    392.219 728.281 870.125 (squared distance: 5085.39)
    437.094 762.938 766.313 (squared distance: 5354.3)
    463.188 814.344 780.063 (squared distance: 6132.54)
    441.531 791.875 745.313 (squared distance: 9430.57)
    465.75 811.781 754.781 (squared distance: 9625.93)
    491.125 677.094 821 (squared distance: 10621.9)
    395.406 670.344 847.875 (squared distance: 11696.3)
    513.594 749.344 930.344 (squared distance: 13525.2)
    447.656 660.813 898.438 (squared distance: 14760.7)
    449.969 763.281 961.281 (squared distance: 14982.1)
    419.344 869.75 776.75 (squared distance: 15085.3)
    380.563 866.75 886.688 (squared distance: 16242)
    492.063 684.906 925.594 (squared distance: 16526.8)
   	//结果是由小到大依次排列

对最大的距离进行验证发现

对16526开平方==128 < 130.57 可以判断所得到的结果都是在规定的搜索半径内的

1.2 八叉树

八叉树相关解释见：https://blog.csdn.net/qq_37855507/article/details/90957798

实现功能

学习使用八叉树在点云数据中进行近邻搜索

  • 体素内近邻搜索
  • K近邻搜索
  • 半径内近邻搜搜

关键API

//[in]表示输入参数， [out]表示输出参数
bool pcl::octree::OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT >::voxelSearch(
    const PointT& point,  				//[in]搜索点
    std::vector<int>& point_idx_data  	//[out]搜索到的近邻带你索引
) 

//k近邻搜索
int pcl::search::Octree< PointT, LeafTWrap, BranchTWrap, OctreeT >::nearestKSearch(
    const PointT& point,  			//[in]搜索点
    int k,  						//[in]近邻点个数
    std::vector<int>& k_indices,  	//[out]搜索到的近邻带你索引
    std::vector<float>& k_sqr_distances  //[out]搜索到的紧邻点的平方距离
)const 


//半径搜搜
int pcl::search::Octree< PointT, LeafTWrap, BranchTWrap, OctreeT >::radiusSearch(
    const PointT& p_q,  			//[in]搜索点
    double radius,  				//[in]搜索半径
    std::vector<int>& k_indices,  	//[out]搜索到的近邻带你索引
    std::vector<float>& k_sqr_distances,  //[out]搜索到的紧邻点的平方距离
    unsigned int max_nn = 0  		//[in]用来限制找到的结果的最大个数，默认为0，即报错所有结果
)const

完整代码

#include <pcl/point_cloud.h>
#include <pcl/octree/octree_search.h>

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

int
main(int argc, char** argv)
{
    srand((unsigned int)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->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);
    }

    float resolution = 256.0f;  //八叉树的分辨率参数，用来描述最低一级八叉树的最小体素的尺寸

    pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);  //初始化八叉树

    octree.setInputCloud(cloud);        //设置输入点云
    octree.addPointsFromInputCloud();   //构建八叉树

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

    // Neighbors within voxel search
    //体素内近邻搜索
    std::vector<int> pointIdxVec;       //用于保存体素近邻搜索的结果向量

    if (octree.voxelSearch(searchPoint, pointIdxVec))
    {
        std::cout << "Neighbors within voxel search at (" << searchPoint.x
            << " " << searchPoint.y
            << " " << searchPoint.z << ")"
            << std::endl;

        for (std::size_t i = 0; i < pointIdxVec.size(); ++i)
            std::cout << "    " << cloud->points[pointIdxVec[i]].x
            << " " << cloud->points[pointIdxVec[i]].y
            << " " << cloud->points[pointIdxVec[i]].z << std::endl;
    }

    // K nearest neighbor search
    //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 (octree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
    {
        for (std::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;
    }

    // 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 (octree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
    {
        for (std::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;
    }
}

运行结果

在这里插入图片描述

在指定的半径内没有搜索到满足条件的点

标签：std,distance,tree,八叉树,squared,points,PCL,近邻,cloud
来源： https://blog.csdn.net/m0_46376148/article/details/120398816

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。

关于我们 | 联系我们 | 留言反馈

专注分享技术，共同学习，共同进步。侵权联系[81616952@qq.com]

Copyright (C)ICode9.com, All Rights Reserved.

ICode9版权所有