点云获取pcl点云以某个点云的已经分块得区域的交集

最新推荐文章于 2024-09-23 11:27:55 发布

再路上1216

最新推荐文章于 2024-09-23 11:27:55 发布

阅读量214

点赞数 3

分类专栏： pcl 文章标签：算法

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

版权

pcl 专栏收录该内容

6 篇文章 0 订阅

订阅专栏

首先将点云分块得到区域后，获取每个块的box的最大最小点云，然后提取box内的点云。

                pcl::IndicesPtr indexes(new pcl::Indices());
                pcl::getPointsInBox(*cloud_1, min_pt, max_pt, *indexes);
                // --------------------------取框内和框外点------------------------------------
                pcl::ExtractIndices<PointType> extr;
                extr.setInputCloud(cloud);  // 设置输入点云
                extr.setIndices(indexes);   // 设置索引
                pcl::PointCloud<PointType>::Ptr cloud_in_box(new pcl::PointCloud<PointType>());
                extr.filter(*cloud_in_box); // 提取对应索引的点云
                cout << "方框内点的个数为：" << cloud_in_box->points.size() << endl;
                pcl::PointCloud<PointType>::Ptr cloud_out_box(new pcl::PointCloud<PointType>);
                extr.setNegative(true);    // 提取对应索引之外的点
                extr.filter(*cloud_out_box);
                cout << "方框外点的个数为：" << cloud_out_box->points.size() << endl;

但是不知道为什么每次提取的indexes都不对，提取的区域大部分都是包含整个点云的点，很奇怪。于是看了下提取box点云的源码

pcl::IndicesPtr indexes(new pcl::Indices());
                pcl::getPointsInBox(*cloud_1, min_pt, max_pt, *indexes);

//
template <typename PointT> inline void
pcl::getPointsInBox (const pcl::PointCloud<PointT> &cloud, 
                     Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt,
                     Indices &indices)
{
  indices.resize (cloud.size ());
  int l = 0;

  // If the data is dense, we don't need to check for NaN
  if (cloud.is_dense)
  {
    for (std::size_t i = 0; i < cloud.size (); ++i)
    {
      // Check if the point is inside bounds
      if (cloud[i].x < min_pt[0] || cloud[i].y < min_pt[1] || cloud[i].z < min_pt[2])
        continue;
      if (cloud[i].x > max_pt[0] || cloud[i].y > max_pt[1] || cloud[i].z > max_pt[2])
        continue;
      indices[l++] = int (i);
    }
  }
  // NaN or Inf values could exist => check for them
  else
  {
    for (std::size_t i = 0; i < cloud.size (); ++i)
    {
      // Check if the point is invalid
      if (!std::isfinite (cloud[i].x) || 
          !std::isfinite (cloud[i].y) || 
          !std::isfinite (cloud[i].z))
        continue;
      // Check if the point is inside bounds
      if (cloud[i].x < min_pt[0] || cloud[i].y < min_pt[1] || cloud[i].z < min_pt[2])
        continue;
      if (cloud[i].x > max_pt[0] || cloud[i].y > max_pt[1] || cloud[i].z > max_pt[2])
        continue;
      indices[l++] = int (i);
    }
  }
  indices.resize (l);
}

不知道为什么不行，但看到只是比价点的大小是否在box内，按照源码修改，完美解决。

//
// Created by wzs on 2024/6/6.
//

#include <vector>
#include <string>
#include <iostream>
#include <sstream>
#include <stdlib.h> //rand()头文件
#include <pcl/io/pcd_io.h>
#include <pcl/octree/octree.h>
#include <boost/thread/thread.hpp>
#include <pcl/visualization/pcl_visualizer.h>

#include <pcl/filters/extract_indices.h>
#include <pcl/common/common.h>
#include "pcl/common/centroid.h"
#include "pcl/common/distances.h"

using namespace std;

typedef pcl::PointXYZ PointType;


int main()
{
    string path("../test_data/1/3.pcd");// 原始点云所在文件夹
    string outpath("../test_data/1/voxel_map");        // 分块保存路径文件夹名

    string path_1("../test_data/1/3_slope.pcd");
    string outpath_1("../test_data/1/voxel_map_1");        // 分块保存路径文件夹名

    string outpath_2("../test_data/1/voxel_map_2");        // 分块保存路径文件夹名

    float resolution = 1.0;        // 设置体素分辨率
    //-------------------------- 加载点云 --------------------------
    pcl::PointCloud<PointType>::Ptr cloud(new pcl::PointCloud<PointType>);
    if (pcl::io::loadPCDFile(path, *cloud) < 0)
    {
        PCL_ERROR("Couldn't read file \n");
//        system("pause");
        return -1;
    }

    pcl::PointCloud<PointType>::Ptr cloud_1(new pcl::PointCloud<PointType>);
    if (pcl::io::loadPCDFile(path_1, *cloud_1) < 0)
    {
        PCL_ERROR("cloud_slopen't read file \n");
//        system("pause");
        return -1;
    }

    // -----------------------获取分块点云保存路径----------------------------------
    printf("开始进行点云分块！！\n");
//    string::size_type iPos = path.find_last_of('//') + 1;
//    string filename = path.substr(iPos, path.length() - iPos);
//    string name = filename.substr(0, filename.rfind("."));
    // ---------------------使用八叉树快速构建体素索引------------------------------
    pcl::octree::OctreePointCloud<PointType> octree(resolution);
    octree.setInputCloud(cloud);
    octree.addPointsFromInputCloud(); // 从点云中构建八叉树
    pcl::Indices pointIdxVec;         // 体素内点的索引
    //-----------------------------开始分块-----------------------------------------
//    boost::shared_ptr<pcl::visualization::PCLVisualizer>viewer(new pcl::visualization::PCLVisualizer("planar segment")); ;
//    viewer->setBackgroundColor(0, 0, 0);
//    viewer->setWindowName("点云分块");
    pcl::PointCloud<PointType>::Ptr seg_cloud(new pcl::PointCloud<PointType>);
//    pcl::PointCloud<PointType>::Ptr seg_cloud_1(new pcl::PointCloud<PointType>);
//    pcl::PointCloud<PointType>::Ptr seg_cloud_1_0(new pcl::PointCloud<PointType>);

    int num_voxel = 0;
    for (auto iter = octree.leaf_breadth_begin(); iter != octree.leaf_breadth_end(); ++iter){
        num_voxel++;
    }
    float mean_voxel_size_ = cloud_1->size() / num_voxel;

    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.clear();
            //从八叉树叶子节点中获取单个叶子容器中的点索引
            pointIdxVec = iter.getLeafContainer().getPointIndicesVector();
            if (pointIdxVec.size() == 0) // 体素内点个数为0则跳过
            {
                continue;
            }else
            {

                seg_cloud->clear();
                
                std::stringstream ss;
                ss << outpath << "//"<< "_block_" << begin + 1 << ".pcd";
                pcl::copyPointCloud(*cloud, pointIdxVec, *seg_cloud);
                pcl::io::savePCDFileBinary(ss.str(), *seg_cloud);
                cout << "第[" << begin + 1 << "]块点云分割完毕！  " << seg_cloud->size() << "    " << mean_voxel_size_ << endl;

                pcl::PointCloud<PointType>::Ptr cloud_in_box(new pcl::PointCloud<PointType>());
                PointType point_min_, point_max_;
                pcl::getMinMax3D(*seg_cloud,point_min_,point_max_);
//                cout << "Min x: " << point_min_.x << endl;
//                cout << "Min y: " << point_min_.y << endl;
//                cout << "Min z: " << point_min_.z << endl;
//                cout << "Max x: " << point_max_.x << endl;
//                cout << "Max y: " << point_max_.y << endl;
//                cout << "Max z: " << point_max_.z << endl;
//                Eigen::Vector4f min_pt = { point_min_.x,point_min_.y,point_min_.z,0 };
//                Eigen::Vector4f max_pt = { point_max_.x,point_max_.y,point_max_.z,0 };
                for (std::size_t i = 0; i < cloud_1->size (); ++i)
                {
                    // Check if the point is inside bounds
                    if (cloud_1->points[i].x < point_min_.x || cloud_1->points[i].y < point_min_.y || cloud_1->points[i].z < point_min_.y)
                        continue;
                    if (cloud_1->points[i].x > point_max_.x || cloud_1->points[i].y > point_max_.y || cloud_1->points[i].z > point_max_.z)
                        continue;
                    cloud_in_box->emplace_back(cloud_1->points[i]);
                }
                cout << "方框内点的个数为：" << cloud_in_box->points.size() << endl;


                std::stringstream ss_1;
                ss_1 << outpath_1 << "//"<< "_block_" << begin + 1 << ".pcd";
                if(cloud_in_box->size() > 0)
                    pcl::io::savePCDFileBinary(ss_1.str(), *cloud_in_box);

                //center distance
                Eigen::Vector4f centroid_seg_, centroid_i_box_;
                PointType centroid_seg_p_, centroid_i_box_p_;
                pcl::compute3DCentroid(*seg_cloud,centroid_seg_);
                pcl::compute3DCentroid(*cloud_in_box,centroid_i_box_);
                centroid_seg_p_.x = centroid_seg_[0];
                centroid_seg_p_.y = centroid_seg_[1];
                centroid_seg_p_.z = centroid_seg_[2];
                centroid_i_box_p_.x = centroid_i_box_[0];
                centroid_i_box_p_.y = centroid_i_box_[1];
                centroid_i_box_p_.z = centroid_i_box_[2];
                float dis_centroid_seg_in_box_ = pcl::euclideanDistance(centroid_seg_p_,centroid_i_box_p_);
                cout << "dis_centroid_seg_in_box_:   " << dis_centroid_seg_in_box_ << endl;
                std::stringstream ss_2;
                ss_2 << outpath_2 << "//"<< "_block_" << begin + 1 << ".pcd";
                if (dis_centroid_seg_in_box_ > 0.1 && cloud_in_box->size() > mean_voxel_size_){
                    pcl::io::savePCDFileBinary(ss_2.str(), *cloud_in_box);
                    cout << begin + 1 << endl;
                }

                     begin++;


            }

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


    printf("点云体素分块完毕！！！");

    return 0;
}

效果图如下：