pcl中octree三种检测方法比较并实现可视化

最新推荐文章于 2022-10-29 09:48:33 发布

原创

最新推荐文章于 2022-10-29 09:48:33 发布 · 1.4k 阅读

7 ·

CC 4.0 BY-SA版权

文章标签：

#pcl #octree

#include <iostream>
#include <pcl/common/common_headers.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/point_cloud.h>
#include <pcl/octree/octree.h>
#include <vector>
#include <ctime>
//pcl::PointCloud<pcl::PointXYZ>::points是vector类型，可以使用push_back添加随机点

const float R = 5; //半径内近邻搜索的半径

int main() {
    std::cout << "Hello, World!" << std::endl;
    srand((unsigned int) time(NULL));
    pcl::PointCloud<pcl::PointXYZ>::Ptr pCloud_in (new pcl::PointCloud<pcl::PointXYZ>);
    //读入点云
    if(pcl::io::loadPCDFile<pcl::PointXYZ>("room_scan1.pcd",*pCloud_in) == -1)
    {
        PCL_ERROR("ERROR Load...");
        return -1;
    }
    std::cout<<"load succeed"<<std::endl;

    float resolution = 128.0f;
    pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);

    octree.setInputCloud(pCloud_in);
    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);
    searchPoint.x = 0.0;
    searchPoint.y = 0.0;
    searchPoint.z = 0.0;

    //体素内近邻搜索
    std::vector<int> pointIdxVec;
    if(octree.voxelSearch(searchPoint,pointIdxVec))
    {
        std::cout<<"Neighbors within voxel search ar ("<<searchPoint.x<<", "<<searchPoint.y<<", "<<searchPoint.z<<")"<<std::endl;
        std::cout<<pointIdxVec.size()<<" points found"<<std::endl;
        for(size_t i=0;i<pointIdxVec.size();i++)