pcl点云 Voxel map

最新推荐文章于 2024-07-18 17:40:16 发布

再路上1216

最新推荐文章于 2024-07-18 17:40:16 发布

阅读量104

点赞数 3

分类专栏： pcl 文章标签： c++

本文链接：https://blog.csdn.net/qq_39003748/article/details/139687314

版权

pcl 专栏收录该内容

6 篇文章 0 订阅

订阅专栏

#include <vector>
#include <string>
#include <iostream>
#include <sstream>
#include <pcl/io/pcd_io.h>
#include <pcl/octree/octree.h>
#include <boost/thread/thread.hpp>


using namespace std;

void cloud_voxel_map(pcl::PointCloud<PointI>::Ptr cloud, float resolution, string out_path){

    // ---------------------使用八叉树快速构建体素索引------------------------------
    pcl::octree::OctreePointCloud<PointI> octree(resolution);
    octree.setInputCloud(cloud);
    octree.addPointsFromInputCloud(); // 从点云中构建八叉树
    pcl::Indices pointIdxVec;         // 体素内点的索引
    //-----------------------------开始分块-----------------------------------------
    pcl::PointCloud<PointI>::Ptr seg_cloud(new pcl::PointCloud<PointI>);
    int begin = 0;
    // 构建八叉树叶子节点迭代器，遍历八叉树
    for (auto iter = octree.leaf_breadth_begin(); iter != octree.leaf_breadth_end(); ++iter)
    {
        auto key = iter.getCurrentOctreeKey(); // 获取当前迭代器八叉树节点的八叉树键
        auto it_key = octree.findLeaf(key.x, key.y, key.z); // 检查是否存在于八叉树中
        if (it_key != nullptr)
        {
            //从八叉树叶子节点中获取单个叶子容器中的点索引
            pointIdxVec = iter.getLeafContainer().getPointIndicesVector();
            if (pointIdxVec.size() < 5) // 体素内点个数为0则跳过
            {
                continue;
            }
            else
            {

                std::stringstream ss;
                ss << out_path<< "block_" << begin << ".pcd";
                string ss_temp = ss.str();
                pcl::copyPointCloud(*cloud, pointIdxVec, *seg_cloud);
                if(seg_cloud->size() >= 5)
                    pcl::io::savePCDFileBinary(ss.str(), *seg_cloud);
                cout << "第[" << begin << "]块点云分割完毕！   " << seg_cloud->size() << endl;
                begin++;

            }

            seg_cloud->clear(); // 每分割一次，清空一下容器，进而提高电脑工作效率
        }
    }
}


int main()
{

    std::string path = (argc > 1) ? argv[1] : "../test_data/smoke_svm/background.pcd";// 原始点云所在文件夹
    std::string outpath = (argc > 2) ? argv[2] : "../test_data/smoke_svm/neg_test/"; // 分块保存路径文件夹名
    float resolution = 1;        // 设置体素分辨率
    //-------------------------- 加载点云 --------------------------
    pcl::PointCloud<PointI>::Ptr cloud(new pcl::PointCloud<PointI>);
    if (pcl::io::loadPCDFile(path, *cloud) < 0)
    {
        PCL_ERROR("Couldn't read file \n");
        system("pause");
        return -1;
    }


    cloud_voxel_map(cloud,resolution,outpath);



	return 0;
}