pcl基于八叉树进行空间划分和搜索操作

        建立空间索引在点云数据处理中已被广泛应用,常见空间索引一般是自顶向下逐级划分空间的各种空间索引结构,比较有代表性的包括 BSP 树、KD 树、KDB 树、 R树、R+树、CELL 树、四叉树和八叉树等索引结构,而在这些结构中 KD 树和八叉树在 3D点云数据排列中应用较为广泛。 PCL 对八叉树的数据结构建立和索引方法进行了实现,以方便在此基础上对点云进行处理操作 。

         八叉树(Octree)的定义是:若不为空树的话,树中任一节点的子节点恰好只会有八个,或零个,也就是子节点不会有0与8以外的数目。那么,这要用来做什么?想象一个立方体,我们最少可以切成多少个相同等分的小立方体?答案就是8个。再想象我们有一个房间,房间里某个角落藏着一枚金币,我们想很快的把金币找出来,聪明的你会怎么做?我们可以把房间当成一个立方体,先切成八个小立方体,然后排除掉没有放任何东西的小立方体,再把有可能藏金币的小立方体继续切八等份….如此下去,平均在log_{8} (n)(n表示房间内的所有物体数)的时间内就可找到金币。因此,八叉树就是用在3D空间中的场景管理,可以很快地知道物体在3D场景中的位置,或侦测与其它物体是否有碰撞以及是否在可视范围内。

这里引入了一个概念:Voxel翻译为体积元素,简称体素。描述了一个预设的最小单位的正方体

        pcl的octree库提供了从点云数据创建具有层次的数据结构的方法。这样就可以对点数据集进行空间分区,下采样和搜索操作。每个八叉树节点有八个子节点或没有子节点。根节点描述了一个包围所有点的3维包容盒子。
        pcl_octree实现提供了有效的最近邻居搜索(邻域搜索)API,例如“ 体素(Voxel)邻居搜索”,“ K最近邻居搜索”和“半径搜索邻居”。叶子节点类也提供其他功能,例如空间“占用率”和“每个体素(Voxel)的点密度”检查;序列化和反序列化功能可将八叉树结构有效地编码为二进制格式;此外,内存池实现减少了昂贵的内存分配和释放操作,以便快速创建八叉树。
        下图说明了最低树级别的八叉树节点的体素边界框。八叉树体素围绕着兔子表面的每个3D点。红点代表点数据。该图像是使用octree_viewer创建的(visualization/tools/octree_viewer

方式二:K 近邻搜索

        本例中K被设置成10, K 近邻搜索(K nearest neighbor search)方法把搜索结果写到两个分开的向量中, 第一个pointIdxNKNSearch 包含搜索结果〈结果点的索引的向量〉, 第二个pointNKNSquaredDistance 保存相应的搜索点和近邻之间的距离平方。 

#include <iostream>
#include <vector>
#include <ctime>
#include <pcl/point_cloud.h>
#include <pcl/octree/octree_search.h>
#include <pcl/visualization/cloud_viewer.h>

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 (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 = 128.0f;  // 设置分辨率为128
    // resolution该参数描述了octree叶子leaf节点的最小体素尺寸。
    pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);
    octree.setInputCloud(cloud);  // 设置输入点云
    octree.addPointsFromInputCloud();  // 通过点云构建octree

    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);
   
    int K = 10;

    std::vector<int> pointIdxNKNSearch;
    std::vector<float> pointNKNSquaredDistance;

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

    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.addCoordinateSystem(200); // 指定坐标轴的长度,单位mm

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

    return 0;
}

 

octree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance)

 pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>::nearestKSearch()方法返回一个整数值,表示搜索操作是否成功并且找到了足够的最近邻点。如果搜索成功并且找到了至少K个最近邻点,则返回实际找到的最近邻点数量;否则,返回0。

该方法将给定的搜索点作为参数,并在八叉树中搜索最近的K个邻近点。找到的最近邻点的索引将存储在pointIdxNKNSearch向量中,而对应的欧氏距离平方将存储在pointNKNSquaredDistance向量中。


方式一:“体素近邻搜索”

        体素近邻搜索(Neighbors within voxel search):它把查询点所在的体素中其他点的索引作为查询结果返回,结果以点索引向量的形式保存,因此搜索点和搜索结果之间的距离取决于八叉树的分辨率参数。

#include <iostream>
#include <vector>
#include <ctime>
#include <pcl/point_cloud.h>
#include <pcl/octree/octree_search.h>
#include <pcl/visualization/cloud_viewer.h>

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 (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 = 128.0f;  // 设置分辨率为128
    // resolution该参数描述了octree叶子leaf节点的最小体素尺寸。
    pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);
    octree.setInputCloud(cloud);  // 设置输入点云
    octree.addPointsFromInputCloud();  // 通过点云构建octree

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

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

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

    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.addCoordinateSystem(200); // 指定坐标轴的长度,单位mm

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

    return 0;
}

 

octree.voxelSearch(searchPoint, pointIdxVec)

 pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>::voxelSearch()方法返回一个bool类型的值,表示搜索操作是否成功。如果搜索成功并且找到了匹配的体素,则返回true;否则,返回false

该方法将给定的搜索点作为参数,并在八叉树中搜索包含该点的体素。如果找到匹配的体素,则会将体素内的点索引存储在pointIdxVec向量中,并返回true。如果没有找到匹配的体素,则pointIdxVec向量将为空,并返回false


 方式三:半径内近邻搜索

         半径内近邻搜索(Neighbors within radius search)原理和“K 近邻搜索”类似,它的搜索结果被写入两个分开的向量中, 这两个向量分别存储结果点的索引和对应的距离平方

#include <iostream>
#include <vector>
#include <ctime>
#include <pcl/point_cloud.h>
#include <pcl/octree/octree_search.h>
#include <pcl/visualization/cloud_viewer.h>

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 (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 = 128.0f;  // 设置分辨率为128
    // resolution该参数描述了octree叶子leaf节点的最小体素尺寸。
    pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);
    octree.setInputCloud(cloud);  // 设置输入点云
    octree.addPointsFromInputCloud();  // 通过点云构建octree

    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;

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

    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.addCoordinateSystem(200); // 指定坐标轴的长度,单位mm

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

    return 0;
}

 

octree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance)

pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>::radiusSearch()方法是用于在八叉树中执行半径搜索的函数。它根据给定的搜索点和半径,在八叉树中找到落在指定半径范围内的所有点。

以下是该方法的参数说明:

  1. searchPoint:要进行半径搜索的点,类型为pcl::PointXYZ。即你希望以这个点为中心进行半径搜索。
  2. radius:指定的搜索半径,类型为double。它定义了从搜索点出发的球体的半径,只有落在该球体内的点才会被搜索到。
  3. pointIdxRadiusSearch:存储在搜索半径范围内的点的索引的向量,类型为std::vector<int>
  4. pointRadiusSquaredDistance:存储在搜索半径范围内的点与搜索点之间的欧氏距离平方的向量,类型为std::vector<float>

使用radiusSearch()方法时,传递一个搜索点和半径作为输入参数,并提供两个向量作为输出参数。方法将填充pointIdxRadiusSearch向量以保存搜索半径范围内的点的索引,同时填充pointRadiusSquaredDistance向量以保存每个点与搜索点之间的欧氏距离平方。

​​​​​​​pcl_octree_viewer工具

pointCloudLibrary点云库在windows下使用pcl_viewer工具

(1)首先在点云库安装目录下找到pcl_octree_viewer.exe,具体根据自己的安装目录确定

(2) 使用cmd命令行打开

 cd 到pcl_octree_viewer.exe 路径,敲入命令:

pcl_octree_viewer.exe F:\DarkHorse\data\rabbit.pcd 0.01

"pcl_octree_viewer.exe <pcd file> <resolution>",其中<pcd file>代表点云文件的路径,<resolution>代表八叉树的分辨率

 

  • v -> 隐藏或显示octree立方体
  • b -> 隐藏或显示体素中心点
  • n -> 隐藏或显示原始点云
  • q -> 退出

 

 

  • 1
    点赞
  • 10
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
这里提供一个基于PCL OutOfCore模块的示例代码,用于加载和显示大规模点云数据: ```cpp #include <pcl/io/pcd_io.h> #include <pcl/visualization/pcl_visualizer.h> #include <pcl/outofcore/outofcore.h> #include <pcl/console/time.h> int main(int argc, char** argv) { // 创建一个 OutOfCoreOctree 对象 pcl::outofcore::OutofcoreOctreeBase<pcl::PointXYZ> octree("pointcloud.octree"); // 从 PCD 文件中加载点云数据 pcl::console::TicToc tt; tt.tic(); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::io::loadPCDFile<pcl::PointXYZ>("pointcloud.pcd", *cloud); std::cout << "Loaded " << cloud->size() << " points in " << tt.toc() << " ms" << std::endl; // 将点云数据添加到 OutOfCoreOctree 中 tt.tic(); octree.convertPointCloud(cloud); std::cout << "Converted " << cloud->size() << " points to octree in " << tt.toc() << " ms" << std::endl; // 创建可视化窗口并显示 OutOfCoreOctree 中的点云数据 pcl::visualization::PCLVisualizer viewer("PointCloud Viewer"); viewer.addCoordinateSystem(); viewer.setBackgroundColor(0.0, 0.0, 0.0); viewer.initCameraParameters(); // 遍历 OutOfCoreOctree 中的所有块,并将其中的点云数据显示出来 std::vector<pcl::outofcore::OutofcoreOctreeBase<pcl::PointXYZ>::OctreeDiskContainer::Ptr> leafs; octree.getLeafContainers(leafs); for (std::vector<pcl::outofcore::OutofcoreOctreeBase<pcl::PointXYZ>::OctreeDiskContainer::Ptr>::iterator it = leafs.begin(); it != leafs.end(); ++it) { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); (*it)->loadPointCloud<pcl::PointXYZ>(cloud); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> handler(cloud, rand() % 256, rand() % 256, rand() % 256); viewer.addPointCloud<pcl::PointXYZ>(cloud, handler); } // 进入可视化循环 viewer.spin(); return 0; } ``` 在这个示例代码中,我们首先创建了一个 OutOfCoreOctreeBase 对象,并将其初始化为一个名为 "pointcloud.octree" 的文件。然后,我们从一个名为 "pointcloud.pcd" 的 PCD 文件中加载点云数据,并使用 convertPointCloud 函数将其添加到 OutOfCoreOctree 中。接着,我们创建了一个可视化窗口,并遍历了 OutOfCoreOctree 中的所有块,将其中的点云数据添加到可视化窗口中进行显示。最后,我们进入了可视化循环,等待用户交互操作

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

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

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值