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

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

pcl基于八叉树进行空间划分和搜索操作_第1张图片

         八叉树(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

pcl基于八叉树进行空间划分和搜索操作_第2张图片

方式二:K 近邻搜索

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

#include 
#include 
#include 
#include 
#include 
#include 

int main(int argc, char** argv) {
    srand((unsigned int)time(NULL));

    pcl::PointCloud::Ptr cloud(new pcl::PointCloud);

    // 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 octree(resolution);
    octree.setInputCloud(cloud);  // 设置输入点云
    octree.addPointsFromInputCloud();  // 通过点云构建octree

    pcl::PointCloud::Ptr searchCloud(new pcl::PointCloud);
    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 pointIdxNKNSearch;
    std::vector 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 originColorHandler(cloud, 255, 255, 255);
    pcl::visualization::PointCloudColorHandlerCustom searchColorHandler(searchCloud, 0, 255, 0);

    pcl::visualization::PCLVisualizer viewer("PCL Viewer");
    viewer.setBackgroundColor(0.1176, 0.1176, 0.2353);
    viewer.addPointCloud(cloud, originColorHandler, "cloud");
    viewer.addPointCloud(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;
}

 pcl基于八叉树进行空间划分和搜索操作_第3张图片

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

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

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


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

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

#include 
#include 
#include 
#include 
#include 
#include 

int main(int argc, char** argv) {
    srand((unsigned int)time(NULL));

    pcl::PointCloud::Ptr cloud(new pcl::PointCloud);

    // 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 octree(resolution);
    octree.setInputCloud(cloud);  // 设置输入点云
    octree.addPointsFromInputCloud();  // 通过点云构建octree

    pcl::PointCloud::Ptr searchCloud(new pcl::PointCloud);
    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 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 originColorHandler(cloud, 255, 255, 255);
    pcl::visualization::PointCloudColorHandlerCustom searchColorHandler(searchCloud, 0, 255, 0);

    pcl::visualization::PCLVisualizer viewer("PCL Viewer");
    viewer.setBackgroundColor(0.1176, 0.1176, 0.2353);
    viewer.addPointCloud(cloud, originColorHandler, "cloud");
    viewer.addPointCloud(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;
}

 pcl基于八叉树进行空间划分和搜索操作_第4张图片

octree.voxelSearch(searchPoint, pointIdxVec)

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

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


 方式三:半径内近邻搜索

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

#include 
#include 
#include 
#include 
#include 
#include 

int main(int argc, char** argv) {
    srand((unsigned int)time(NULL));

    pcl::PointCloud::Ptr cloud(new pcl::PointCloud);

    // 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 octree(resolution);
    octree.setInputCloud(cloud);  // 设置输入点云
    octree.addPointsFromInputCloud();  // 通过点云构建octree

    pcl::PointCloud::Ptr searchCloud(new pcl::PointCloud);
    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 pointIdxRadiusSearch;
    std::vector 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 originColorHandler(cloud, 255, 255, 255);
    pcl::visualization::PointCloudColorHandlerCustom searchColorHandler(searchCloud, 0, 255, 0);

    pcl::visualization::PCLVisualizer viewer("PCL Viewer");
    viewer.setBackgroundColor(0.1176, 0.1176, 0.2353);
    viewer.addPointCloud(cloud, originColorHandler, "cloud");
    viewer.addPointCloud(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;
}

 pcl基于八叉树进行空间划分和搜索操作_第5张图片

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

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

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

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

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

​​​​​​​pcl_octree_viewer工具

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

(1)首先在点云库安装目录下找到pcl_octree_viewer.exe,具体根据自己的安装目录确定
pcl基于八叉树进行空间划分和搜索操作_第6张图片

(2) 使用cmd命令行打开

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

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

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

pcl基于八叉树进行空间划分和搜索操作_第7张图片

 

  • v -> 隐藏或显示octree立方体
  • b -> 隐藏或显示体素中心点
    pcl基于八叉树进行空间划分和搜索操作_第8张图片
  • n -> 隐藏或显示原始点云
    pcl基于八叉树进行空间划分和搜索操作_第9张图片
  • q -> 退出

 

 

你可能感兴趣的:(PCL,pcl)