PCL聚类和最小包围盒AABB

//实现点云聚类和最小包围盒算法
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
//VTK_MODULE_INIT(vtkRenderingOpenGL);
//VTK_MODULE_INIT(vtkInteractionStyle);
#include 
#include 
#include 
#include 
#include 
#include 

int *rand_rgb(){//随机产生颜色
    int *rgb = new int[3];
    rgb[0] = rand() % 255;
    rgb[1] = rand() % 255;
    rgb[2] = rand() % 255;
    return rgb;
}
typedef pcl::PointXYZ PointT;
typedef pcl::PointXYZRGBA Pointcolor;

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCLPointCloud2* clouddd = new pcl::PCLPointCloud2;    //原始的点云的数据格式

int main(int argc, char* argv[])
{
    QCoreApplication a(argc, argv);
    //读入数据

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile("./cloud_cluster_0.pcd", *cloud) == -1)
    {
         PCL_ERROR("Cloudn't read file!");
         system("pause");
         return -1;
    }
    std::cout << "Reader Normal" << std::endl;
    std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl;
    /*滤波部分*/
    //1Z轴阈值
    pcl::PassThrough<PointT> pass;
    pass.setInputCloud (cloud);
    pass.setFilterFieldName ("z");
    pass.setFilterLimits (0, 100);
    pass.filter (*cloud);
    //1cm下采样
    pcl::VoxelGrid<pcl::PointXYZ> vg;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
    vg.setInputCloud (cloud);
    vg.setLeafSize (0.01f, 0.01f, 0.01f);
    vg.filter (*cloud_filtered);
    //移除离群点 创建滤波器对象
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filteredyc (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
    sor.setInputCloud (cloud_filtered);
    sor.setMeanK (25);
    sor.setStddevMulThresh (1.0);
    sor.filter (*cloud_filteredyc);
    //构建树
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
    tree->setInputCloud(cloud_filteredyc);
    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
    ec.setClusterTolerance (1.5); //设置临近搜索半径
    ec.setMinClusterSize (10);    //设置最小点
    ec.setMaxClusterSize (1000);  //设置最多点
    ec.setSearchMethod (tree);    //设置点云的搜索机制
    ec.setInputCloud (cloud_filteredyc);
    ec.extract (cluster_indices);//索引记录
    pcl::PointCloud<Pointcolor>::Ptr cloudclour;
    //show
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer->setBackgroundColor(0,0,0);
    viewer->addCoordinateSystem(1.0);
    viewer->initCameraParameters();
    std::string index_num;
    int index_num_int = 0;
    for(auto x:cluster_indices)
    {
        std::stringstream ssTemp;
        ssTemp<<index_num_int++;
        index_num=ssTemp.str();
        typename pcl::PointCloud<PointT>::Ptr cloud_temp(new pcl::PointCloud<PointT>);
        for(auto p:x.indices)
           cloud_temp->points.push_back(cloud_filteredyc->points[p]);
        int *rgb1 = rand_rgb();
        //
        pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
        feature_extractor.setInputCloud(cloud_temp);
        feature_extractor.compute();

        std::vector <float> moment_of_inertia;
        std::vector <float> eccentricity;
        pcl::PointXYZ min_point_AABB;
        pcl::PointXYZ max_point_AABB;
        float major_value, middle_value, minor_value;
        Eigen::Vector3f major_vector, middle_vector, minor_vector;
        Eigen::Vector3f mass_center;

        feature_extractor.getMomentOfInertia(moment_of_inertia);
        feature_extractor.getEccentricity(eccentricity);
        feature_extractor.getAABB(min_point_AABB, max_point_AABB);
        feature_extractor.getEigenValues(major_value, middle_value, minor_value);
        feature_extractor.getEigenVectors(major_vector, middle_vector, minor_vector);
        feature_extractor.getMassCenter(mass_center);
        //aabb外接立方体
        viewer->addCube(min_point_AABB.x, max_point_AABB.x, min_point_AABB.y, max_point_AABB.y, min_point_AABB.z, max_point_AABB.z, rgb1[0],rgb1[1], rgb1[2], "AABB"+index_num);
        viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY,0.1, "AABB"+index_num);
        viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH,4, "AABB"+index_num);
        //
        viewer->addPointCloud<pcl::PointXYZ>(cloud_temp,pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(cloud_temp, rgb1[0],rgb1[1], rgb1[2]), "sample cloud"+index_num);
        delete[]rgb1;
    }
    while (!viewer->wasStopped())
    {
         viewer->spinOnce(100);
         boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }

    return (0);
}

效果图