利用pcl库实现简单单帧障碍物检测

利用pcl库实现简单单帧障碍物检测

#include <iostream>
#include <pcl/common/common_headers.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/console/parse.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/crop_box.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/filters/extract_indices.h>
#include <string>
using namespace std;

//聚类函数
std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> Clustering(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, float clusterTolerance, int minSize, int maxSize)
{

    //创建一个tree对象
    pcl::search::KdTree<pcl::PointXYZI>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZI>); //创建一个KDtree
    //将点云数据按照tree的方式进行存储，方便后续遍历
    tree->setInputCloud(cloud);

    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZI> ec;
    ec.setClusterTolerance(clusterTolerance);
    ec.setMinClusterSize(minSize); //设置一个类别里面最小点云数
    ec.setMaxClusterSize(maxSize); //设置一个类别里面最大点云数
    ec.setSearchMethod(tree);      //采用KDtree的寻找方法
    ec.setInputCloud(cloud);
    ec.extract(cluster_indices);

    std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> clusters; //创建一个保存点云指针的容器
    //这个容器里面保存的都是聚集的小类点云
    for (pcl::PointIndices getIndices : cluster_indices)
    {
        typename pcl::PointCloud<pcl::PointXYZI>::Ptr cloudCluster(new pcl::PointCloud<pcl::PointXYZI>);
        for (int index : getIndices.indices)
            cloudCluster->points.push_back(cloud->points[index]);
        cloudCluster->width = cloudCluster->points.size();
        cloudCluster->height = 1;
        cloudCluster->is_dense = true;
        clusters.push_back(cloudCluster);
    }

    return clusters;
}

struct Box //创建一个红色的矩形框
{
    float x_min;
    float y_min;
    float z_min;
    float x_max;
    float y_max;
    float z_max;
};

Box BoundingBox(pcl::PointCloud<pcl::PointXYZI>::Ptr cluster)
{

    // Find bounding box for one of the clusters
    pcl::PointXYZI minPoint, maxPoint;
    pcl::getMinMax3D(*cluster, minPoint, maxPoint);

    Box box;
    box.x_min = minPoint.x;
    box.y_min = minPoint.y;
    box.z_min = minPoint.z;
    box.x_max = maxPoint.x;
    box.y_max = maxPoint.y;
    box.z_max = maxPoint.z;

    return box;
}

int main(int argc, char **argv)
{
    std::cout << "Test PCL !!!" << std::endl;
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::io::loadPCDFile(argv[1], *cloud);
    cout << "滤波前点数量：" << cloud->size() << endl;

    pcl::VoxelGrid<pcl::PointXYZI> sor;
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloudFiltered(new pcl::PointCloud<pcl::PointXYZI>);
    sor.setInputCloud(cloud);
    sor.setLeafSize(0.1f, 0.1f, 0.1f);
    sor.filter(*cloudFiltered); //体素滤波之后的数据保存在cloudFiltered里面
    cout << "滤波后点数量：" << cloudFiltered->size() << endl;

    //下面操作会保留箱子内的点
    // typename pcl::PointCloud<pcl::PointXYZI>::Ptr cloudRegion(new pcl::PointCloud<pcl::PointXYZI>);
    // pcl::CropBox<pcl::PointXYZI> region(true);
    // region.setMin(Eigen::Vector4f(-1.5, -1.7, -1, 1));
    // region.setMax(Eigen::Vector4f(2.6, 1.7, -0.4, 1));
    // region.setInputCloud(cloudFiltered);
    // region.filter(*cloudRegion);

    pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
    // TODO:: Fill in this function to find inliers for the cloud.
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
    pcl::SACSegmentation<pcl::PointXYZI> seg;
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setMaxIterations(1000);
    seg.setDistanceThreshold(0.2);
    seg.setInputCloud(cloudFiltered);
    seg.segment(*inliers, *coefficients); // inliers里面保存的估计是平面点
    if (inliers->indices.size() == 0)
    {
        std::cout << "could not find a plane model for the given dataset." << std::endl;
    }

    pcl::PointCloud<pcl::PointXYZI>::Ptr planeOrObsCloudPtr(new pcl::PointCloud<pcl::PointXYZI>);
    //提取平面点，下面如果设置为tree的话可能是提取除了平面所有的点
    pcl::ExtractIndices<pcl::PointXYZI> extract;
    extract.setInputCloud(cloudFiltered);
    extract.setIndices(inliers);
    extract.setNegative(true);           //提取传入索引外的点，false就是默认，会提取默认索引内的点
    extract.filter(*planeOrObsCloudPtr); //提取的结果

    //创建一个容器用来保存聚类之后的点云
    std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> VecAftClu = Clustering(planeOrObsCloudPtr, 0.5, 50, 10000);
    cout << "一共聚类为多少簇：" << VecAftClu.size() << endl;
    std::vector<Box> VecBox;
    for (int i = 0; i < VecAftClu.size(); i++)
    {
        VecBox.push_back(BoundingBox(VecAftClu[i])); //获取每一个聚类的矩形框
    }

    // AABB外接立方体
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer->setBackgroundColor(0, 0, 0);
    viewer->addCoordinateSystem(0.1);
    viewer->initCameraParameters();

    //根据点云的intensity给点云上色
    pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI> intensity_distribution(cloud, "intensity");
    viewer->addPointCloud<pcl::PointXYZI>(cloud, intensity_distribution, "sample cloud");

    //直接显示普通的点云则就根据下面
    // viewer->addPointCloud<pcl::PointXYZI>(planeOrObsCloudPtr, "sample cloud");

    std::vector<std::string> name;
    for (int i = 0; i < VecBox.size(); i++)
    {
        name.push_back(to_string(i));
    }

    for (int i = 0; i < VecBox.size(); i++)
    {
       

        //下面是设置目标包围框的一些属性，包括包围框的颜色，包围框的不透明度等等

        //下面设置的是包围框的线条的属性
         viewer->addCube(VecBox[i].x_min, VecBox[i].x_max, VecBox[i].y_min, VecBox[i].y_max, VecBox[i].z_min, VecBox[i].z_max, 1.0, 0.0, 0.0, to_string(i), 0);
        //第一句话表示添加的立方体只显示线框
        viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, to_string(i));
        //设置立方体的颜色属性
        viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, to_string(i));
        //设置立方体的不透明度属性
        viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5, to_string(i)); //设置不透明度这个属性为0.5


        //下面是设置包围框面的属性
        std::string cubeFill = "boxFill" + std::to_string(i);
        // viewer->addCube(box.bboxTransform, box.bboxQuaternion, box.cube_length, box.cube_width, box.cube_height, cubeFill);
        viewer->addCube(VecBox[i].x_min, VecBox[i].x_max, VecBox[i].y_min, VecBox[i].y_max, VecBox[i].z_min, VecBox[i].z_max, 1.0, 0.0, 0.0, cubeFill);
        viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_SURFACE, cubeFill);//surface表示表面
        viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, cubeFill);//表面的颜色
        viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5 * 0.3, cubeFill);//表面的不透明度
    }
    

    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }

    // pcl::visualization::CloudViewer viewer("test");
    // viewer.showCloud(planeOrObsCloudPtr);
    // while (!viewer.wasStopped())
    // {
    // };
    return 0;
}

CMakeLists.txt文件

cmake_minimum_required(VERSION 2.6 FATAL_ERROR)
project(pcl_test)

find_package(PCL 1.2 REQUIRED)
#include_directories( "/usr/include/eigen3")

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable(pcl_test pcl_test.cpp)
target_link_libraries (pcl_test ${PCL_LIBRARIES})

install(TARGETS pcl_test RUNTIME DESTINATION bin)

结果显示
懒得去调整里面的一些参数了，主要是为了学习pcl库，来实现简单的小demo，本文的代码主要是根据一个网上的代码改动的，具体哪一个找不到了。