#include "PipesSetCloud.h"
#include <pcl/filters/uniform_sampling.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <thread>
#include <mutex>

#define PASS_THROUGH_INPUT_CLOUD OriginalCloud
#define DOWN_SAMPLE_INPUT_CLOUD1 PassThroughFilteredCloud
#define DOWN_SAMPLE_INPUT_CLOUD2 StatisticalOutlierFilteredCloud
#define STATISTICAL_OUTLIER_INPUT_CLOUD PassThroughFilteredCloud
#define DIR_FILTER_INPUT_CLOUD PassThroughFilteredCloud
#define SEGMENT_INPUT_CLOUD DirFilteredCloud
#define SEGMENT_INPUT_NORMALS_PTR pDirFilteredCloudNormals

std::mutex mtx;

static PointCloud<PointXYZ> OriginalCloud;
static PointCloud<PointXYZ> DownSampledCloud1;//线程1降采样后的点云
static PointCloud<PointXYZ> DownSampledCloud2;//线程2降采样后的点云
static PointCloud<PointXYZ> PassThroughFilteredCloud;
static PointCloud<PointXYZ> StatisticalOutlierFilteredCloud;
static PointCloud<PointXYZ> DirFilteredCloud;

// 输出点云对象，用于存储估计的整体法向量
static pcl::PointCloud<pcl::Normal>::Ptr cloud_normals1(new pcl::PointCloud<pcl::Normal>);
static pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2(new pcl::PointCloud<pcl::Normal>);
static pcl::Normal main_normals = { 0, 0, 0, 0 };//主方向法向量
static pcl::PointCloud<pcl::Normal>::Ptr pDirFilteredCloudNormals(new pcl::PointCloud<pcl::Normal>);
bool IsMainNormalCalculatedFlag = false;

static void print_message(std::string message) {
    // Lock the mutex
    mtx.lock();

    // Print the message
    std::cout << message << endl;

    // Unlock the mutex
    mtx.unlock();
}

template<typename T>
static pcl::PointCloud<T> CloudSubtraction(const std::shared_ptr<pcl::PointCloud<T>>& cloudA, std::shared_ptr<pcl::PointCloud<T>>& cloudB) {
    pcl::search::KdTree<T>::Ptr kdtree(new pcl::search::KdTree<T>);
    kdtree->setInputCloud(cloudB);

    std::vector<int> pointIdxRadiusSearch;
    std::vector<float> pointRadiusSquaredDistance;
    std::vector<int> indices;
    Indices k_indices;
    float radius = 0.001f;
    
    for (size_t i = 0; i < cloudA->size(); i++)
    {

        //搜索CloudB中距离cloudA->pionts[i]最近的点，距离小于radius的点的索引保存在pointIdxRadiusSearch中
        if (kdtree->radiusSearch(cloudA->points[i], radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
        {
            //将cloudA->points[i]从输出点云中删除
            indices.push_back(i);
        }
    }

    pcl::PointCloud<T>::Ptr cloud_diff(new pcl::PointCloud<T>);
    for (size_t i = 0; i < cloudA->size(); i++)
    {
        if (find(indices.begin(), indices.end(), i) == indices.end())
            cloud_diff->push_back(cloudA->points[i]);
    }

    return *cloud_diff;
}

static float calculateAngleBetweenNormals(const pcl::Normal& normal1, const pcl::Normal& normal2) {
    // 计算法向量之间的点积
    float dot_product = normal1.normal_x * normal2.normal_x +
        normal1.normal_y * normal2.normal_y +
        normal1.normal_z * normal2.normal_z;

    // 计算角度
    float angle = std::acos(dot_product);

    // 转换角度为度数
    float angle_in_degrees = angle * 180.0 / M_PI;

    return angle_in_degrees;
}

PipesSetCloud::PipesSetCloud(const PointCloud<PointXYZ>::Ptr& pcloud) {
    this->points = pcloud->points;
    this->header = pcloud->header;
    this->width = pcloud->width;
    this->height = pcloud->height;
    this->is_dense = pcloud->is_dense;
    if (OriginalCloud.points.size() == 0){
        OriginalCloud = *pcloud;
    }
}

PipesSetCloud::PipesSetCloud(const PointCloud<PointXYZ>& cloud) {
    this->points = cloud.points;
    this->header = cloud.header;
    this->width = cloud.width;
    this->height = cloud.height;
    this->is_dense = cloud.is_dense;
    if (OriginalCloud.points.size() == 0) {
        OriginalCloud = cloud;
    }
}

//线程1：降采样->计算小尺度法线
static void Thread1(PipesSetCloud* pipesSetCloud, int KSearch1, float fRadiusSearch1)
{
    //降采样
    pipesSetCloud->DownSample1(fRadiusSearch1);
    mtx.lock();
    std::cout << "After thread 1 downsampling, the point cloud has " \
        << pipesSetCloud->getDownSampledCloud1().points.size() \
        << "points and filters out a total of " \
        << (pipesSetCloud->getPassThroughFilteredCloud().points.size() - pipesSetCloud->getDownSampledCloud1().points.size())\
        << "points" << std::endl;
    mtx.unlock();
    // 创建法向量估计对象
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
    ne.setInputCloud(std::make_shared<PointCloud <PointXYZ>>(pipesSetCloud->getDownSampledCloud1()));

    // 创建一个 kd-tree 对象，用于搜索
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    ne.setSearchMethod(tree);

    // 设置搜索半径或最近邻点数
    //ne.setRadiusSearch(0.03);  // 使用搜索半径
    ne.setKSearch(KSearch1);       // 或者使用最近邻点数

    // 计算法向量
    ne.compute(*cloud_normals1);
    mtx.lock();
    std::cout << "Thread 1 completes the normal calculation process for each point after downsampling" << std::endl;
    mtx.unlock();
}

//线程2：统计滤波->降采样->计算整体法线
static void Thread2(PipesSetCloud* pipesSetCloud, int KSearch, int mean_k, double std_dev_mul_thresh, float fRadiusSearch2){
    pcl::PointCloud<pcl::Normal> normals = pcl::PointCloud<pcl::Normal>();
    auto pnormals = std::make_shared<PointCloud<pcl::PointXYZ>>();
    auto pdiff_normals = std::make_shared<PointCloud<pcl::Normal>>();
    pipesSetCloud->StatisticalOutlierRemoval(mean_k, std_dev_mul_thresh);
    mtx.lock();
    std::cout << "Thread 2 Statistical filtering is complete. After this filtering, the number of point clouds is "<< pipesSetCloud->getStatisticalOutlierFilteredCloud().points.size() << " and the number of abnormal points is "<< (pipesSetCloud->getPassThroughFilteredCloud().points.size() - pipesSetCloud->getStatisticalOutlierFilteredCloud().points.size()) << " ." << std::endl;
    mtx.unlock();
    pipesSetCloud->DownSample2(fRadiusSearch2);
    mtx.lock();
    std::cout << "After downsampling in thread 2, there are " << DownSampledCloud2.points.size() << " points in the point cloud, and a total of " << pipesSetCloud->getStatisticalOutlierFilteredCloud().points.size() - pipesSetCloud->getDownSampledCloud2().points.size() << " points are eliminated." << std::endl;
    mtx.unlock();
    // 创建法向量估计对象
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
    ne.setInputCloud(std::make_shared<PointCloud <PointXYZ>>(pipesSetCloud->getDownSampledCloud2()));

    // 创建一个 kd-tree 对象，用于搜索
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    ne.setSearchMethod(tree);

    // 设置搜索半径或最近邻点数
    //ne.setRadiusSearch(0.03);  // 使用搜索半径
    ne.setKSearch(KSearch);       // 或者使用最近邻点数

    // 计算法向量
    ne.compute(*cloud_normals2);
    mtx.lock();
    std::cout << "The process of calculating the normal direction of each point after statistical filter downsampling is completed in thread 2." << std::endl;
    mtx.unlock();
    normals = *cloud_normals2;
    //计算主方向
    //遍历cloud_normals2
    float angle;
    for (int i = 0; i < normals.points.size(); i++) {
        //如果cloud_normals2中点的方向与主方向法向量的角度大于于20度，则剔除该点
        angle = calculateAngleBetweenNormals(normals.points[i], agreed_normals_param);
        if ((angle > 20) && angle < 160) {
            //pdelete_normals->points.push_back(PointXYZ(normals.points[i].normal_x, normals.points[i].normal_y, normals.points[i].normal_z));
        }
        else if(angle >= 160){
            normals.points[i].normal_x *= -1;
            normals.points[i].normal_y *= -1;
            normals.points[i].normal_z *= -1;
            pdiff_normals->points.push_back(normals.points[i]);
        }
        else if (angle <= 20) {
            pdiff_normals->points.push_back(normals.points[i]);
        }
    }
    //计算normals的质心 
    //计算主方向法向量
    //遍历cloud_normals2
    float sum_x = 0, sum_y = 0, sum_z = 0;
    for (int i = 0; i < pdiff_normals->points.size(); i++) {
        sum_x += pdiff_normals->points[i].normal_x;
        sum_y += pdiff_normals->points[i].normal_y;
        sum_z += pdiff_normals->points[i].normal_z;
    }
    main_normals.normal_x = sum_x / pdiff_normals->points.size();
    main_normals.normal_y = sum_y / pdiff_normals->points.size();
    main_normals.normal_z = sum_z / pdiff_normals->points.size();
    //计算主方向法向量的曲率
    try {
        main_normals.curvature = 1;
    }
    catch (const std::runtime_error& e) {
        std::cerr << e.what() << std::endl;
    }
    mtx.lock();
    std::cout << "Calculate the principal normal vector: " << std::endl;
    std::cout <<"normal_x: " << main_normals.normal_x << std::endl; 
    std::cout <<"normal_y: " << main_normals.normal_y << std::endl; 
    std::cout <<"normal_z: " << main_normals.normal_z << std::endl;
    mtx.unlock();
    IsMainNormalCalculatedFlag = true;
}

//线程1降采样函数
PointCloud<PointXYZ>& PipesSetCloud::DownSample1(double fRadiusSearch) {
    if (DOWN_SAMPLE_INPUT_CLOUD1.points.size() == 0)
    {
        throw std::runtime_error("Input cloud is empty!");
    }
    PointCloud<PointXYZ>::Ptr pInputCloud = std::make_shared<PointCloud<PointXYZ>>(DOWN_SAMPLE_INPUT_CLOUD1);
    pcl::UniformSampling<pcl::PointXYZ> us;
    us.setInputCloud(pInputCloud);
    us.setRadiusSearch(fRadiusSearch); // 设置搜索半径
    us.filter(DownSampledCloud1);
    return DownSampledCloud1;
}

//线程2降采样
PointCloud<PointXYZ>& PipesSetCloud::DownSample2(double fRadiusSearch) {
    if (DOWN_SAMPLE_INPUT_CLOUD2.points.size() == 0)
    {
        throw std::runtime_error("Input cloud is empty!");
    }
    PointCloud<PointXYZ>::Ptr pInputCloud = std::make_shared<PointCloud<PointXYZ>>(DOWN_SAMPLE_INPUT_CLOUD2);
    pcl::UniformSampling<pcl::PointXYZ> us;
    us.setInputCloud(pInputCloud);
    us.setRadiusSearch(fRadiusSearch); // 设置搜索半径
    us.filter(DownSampledCloud2);
    return DownSampledCloud2;
}

PointCloud<PointXYZ>& PipesSetCloud::PassThroughFilter(float x_min, float x_max, float y_min, float y_max, float z_min, float z_max) {
    if (PASS_THROUGH_INPUT_CLOUD.points.size() == 0)
    {
        throw std::runtime_error("Input cloud is empty!");
    }
    auto pInputCloud = std::make_shared<PointCloud<PointXYZ>>(PASS_THROUGH_INPUT_CLOUD);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_X_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_Y_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    // pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_Z_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    // 创建直通滤波器对象
    pcl::PassThrough<pcl::PointXYZ> pass;

    // 设置输入点云
    pass.setInputCloud(pInputCloud);

    // 设置滤波字段为X轴，并设置范围
    pass.setFilterFieldName("x");
    pass.setFilterLimits(x_min, x_max); // 设置X轴的范围
    pass.filter(*cloud_X_filtered);

    // 更新输入点云为滤波后的点云
    pass.setInputCloud(cloud_X_filtered);

    // 设置滤波字段为Y轴，并设置范围
    pass.setFilterFieldName("y");
    pass.setFilterLimits(y_min, y_max); // 设置Y轴的范围
    pass.filter(*cloud_Y_filtered);

    // 更新输入点云为滤波后的点云
    pass.setInputCloud(cloud_Y_filtered);

    // 设置滤波字段为Z轴，并设置范围
    pass.setFilterFieldName("z");
    pass.setFilterLimits(z_min, z_max); // 设置Z轴的范围
    pass.filter(PassThroughFilteredCloud);
    return PassThroughFilteredCloud;
}

PointCloud<PointXYZ>& PipesSetCloud::StatisticalOutlierRemoval(int mean_k, double std_dev_mul_thresh) {
    if (STATISTICAL_OUTLIER_INPUT_CLOUD.points.size() == 0)
    {
        throw std::runtime_error("Input cloud is empty!");
    }
    auto pInutCloud = std::make_shared<PointCloud<PointXYZ>>(STATISTICAL_OUTLIER_INPUT_CLOUD);
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
    sor.setInputCloud(pInutCloud);
    sor.setMeanK(mean_k); // 设置平均邻域数目
    sor.setStddevMulThresh(std_dev_mul_thresh); // 设置标准差倍数阈值
    sor.filter(StatisticalOutlierFilteredCloud);
    return StatisticalOutlierFilteredCloud;
}

PointCloud<PointXYZ>& PipesSetCloud::getOriginalCloud() {
    if (OriginalCloud.points.size() == 0)
    {
        throw std::runtime_error("Original cloud is empty!");
    }
    return OriginalCloud;
}

PointCloud<PointXYZ>& PipesSetCloud::getDownSampledCloud1() {
    if (DownSampledCloud1.points.size() == 0)
    {
        throw std::runtime_error("DownSampled1 cloud is empty!");
    }
    return DownSampledCloud1;
}

PointCloud<PointXYZ>& PipesSetCloud::getDownSampledCloud2() {
    if (DownSampledCloud2.points.size() == 0)
    {
        throw std::runtime_error("DownSampled2 cloud is empty!");
    }
    return DownSampledCloud2;
}

PointCloud<PointXYZ>& PipesSetCloud::getPassThroughFilteredCloud() {
    if (PassThroughFilteredCloud.points.size() == 0)
    {
        throw std::runtime_error("PassThroughFiltered cloud is empty!");
    }
    return PassThroughFilteredCloud;
}

PointCloud<PointXYZ>& PipesSetCloud::getStatisticalOutlierFilteredCloud() {
    if (StatisticalOutlierFilteredCloud.points.size() == 0)
    {
        throw std::runtime_error("StatisticalOutlierFiltered cloud is empty!");
    }
    return StatisticalOutlierFilteredCloud;
}

void PipesSetCloud::DisplayCloud(EM_CLOUD_TYPE emCloudType) {
    PointCloud<PointXYZ>::Ptr pCloud;
    switch (emCloudType) {
    case EM_CLOUD_TYPE_ORIGINAL:
        pCloud = std::make_shared<PointCloud<PointXYZ>>(OriginalCloud);
        break;
    case EM_CLOUD_TYPE_DOWN_SAMPLED:
        pCloud = std::make_shared<PointCloud<PointXYZ>>(DownSampledCloud2);
        break;
    case EM_CLOUD_TYPE_PASS_THROUGH_FILTERED:
        pCloud = std::make_shared<PointCloud<PointXYZ>>(PassThroughFilteredCloud);
        break;
    case EM_CLOUD_TYPE_STATISTICAL_OUTLIER_FILTERED:
        pCloud = std::make_shared<PointCloud<PointXYZ>>(StatisticalOutlierFilteredCloud);
        break;
    case EM_CLOUD_TYPE_DIR_FILLED:
        pCloud = std::make_shared<PointCloud<PointXYZ>>(DirFilteredCloud);
        break;
    default:
        break;
    }

#define VIEW_CLOUD1 pCloud
    //#define VIEW_CLOUD2 RealCloudLeft
    //#define VIEW_CLOUD3 transformed
    pcl::visualization::PCLVisualizer viewer("Cloud Viewer");
    // 添加坐标轴
#ifndef UNIT_CLOUD
    viewer.addCoordinateSystem(0.1); // 参数表示坐标轴的长度，单位：mm,红色为x轴,绿色为y轴,蓝色为z轴
#else
    viewer.addCoordinateSystem(0.1 * UNIT_CLOUD); // 参数1.0表示坐标轴的长度，单位：mm,红色为x轴,绿色为y轴,蓝色为z轴
#endif
    // 添加点云到viewer中，命名为"cloud"
    viewer.addPointCloud<PointType>(VIEW_CLOUD1, "cloud1");
#ifdef VIEW_CLOUD2
    viewer.addPointCloud<PointType>(VIEW_CLOUD2, "cloud2");
#endif
#ifdef VIEW_CLOUD3
    viewer.addPointCloud<PointType>(VIEW_CLOUD3, "cloud3");
#endif
    // 创建一个自定义颜色处理器，将点云颜色设置为红色
    pcl::visualization::PointCloudColorHandlerCustom<PointType> colorHandler1(VIEW_CLOUD1, 255, 0, 0);//红
#ifdef VIEW_CLOUD2
    pcl::visualization::PointCloudColorHandlerCustom<PointType> colorHandler2(VIEW_CLOUD2, 0, 255, 0);//绿
#endif
#ifdef VIEW_CLOUD3
    pcl::visualization::PointCloudColorHandlerCustom<PointType> colorHandler3(VIEW_CLOUD3, 0, 0, 255);//蓝
#endif
    // 添加带颜色的点云到viewer中，命名为"colored cloud"
    viewer.addPointCloud<PointType>(VIEW_CLOUD1, colorHandler1, "colored cloud1");
#ifdef VIEW_CLOUD2
    viewer.addPointCloud<PointType>(VIEW_CLOUD2, colorHandler2, "colored cloud2");
#endif
#ifdef VIEW_CLOUD3
    viewer.addPointCloud<PointType>(VIEW_CLOUD3, colorHandler3, "colored cloud3");
#endif
    // 设置点云的渲染属性，点的大小为1
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 0.5, "colored cloud1");
#ifdef VIEW_CLOUD2
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 0.5, "colored cloud2");
#endif
#ifdef VIEW_CLOUD3
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 0.5, "colored cloud3");
#endif
    // 设置viewer的背景颜色为灰色
    viewer.setBackgroundColor(100, 100, 100);
    //主循环
    while (!viewer.wasStopped()) {
        // 更新viewer
        viewer.spinOnce(100);
    }
}

PointCloud<PointXYZ>& PipesSetCloud::DirFiler(int KSearch1, double fRadiusSearch1, int KSearch2, int mean_k2, double std_dev_mul_thresh2,  float fRadiusSearch2) {
    float angle = 0.0f;
    pcl::PointCloud<pcl::Normal> normals;
    if (DIR_FILTER_INPUT_CLOUD.points.size() == 0)
    {
        throw std::runtime_error("Input cloud is empty!");
    }
    auto pInputCloud = std::make_shared<PointCloud<PointXYZ>>(DIR_FILTER_INPUT_CLOUD);
    // 创建线程对象
    std::thread t1(Thread1, this, KSearch1, (float)fRadiusSearch1);
    std::thread t2(Thread2, this, KSearch2, mean_k2, std_dev_mul_thresh2, (float)fRadiusSearch2);
    t1.join();
    t2.join();
    
    while (cloud_normals1->points.size() == 0 || !IsMainNormalCalculatedFlag) {};
    std::cout << "Start direction filtering, a total of " << DownSampledCloud1.points.size() << " points before filtering." << std::endl;
    normals = *cloud_normals1;
    // 遍历cloud_normals1
    for (int i = 0; i < normals.points.size(); i++) {
        //如果cloud_normals1中点的方向与主方向法向量的角度大于于20度，则剔除该点
        angle = calculateAngleBetweenNormals(normals.points[i], main_normals);
        if ((angle > 20) && angle < 160) {
        }
        else if (angle >= 160) {
            normals.points[i].normal_x *= -1;
            normals.points[i].normal_y *= -1;
            normals.points[i].normal_z *= -1;
            DirFilteredCloud.points.push_back(DownSampledCloud1.points[i]);
            pDirFilteredCloudNormals->points.push_back(normals.points[i]);
        }
        else if (angle <= 20) {
            DirFilteredCloud.points.push_back(DownSampledCloud1.points[i]);
            pDirFilteredCloudNormals->points.push_back(normals.points[i]);
        }
    }
    std::cout << "After directional filtering, there are" << DirFilteredCloud.points.size() << " 50 points in total, and" << DownSampledCloud1.points.size() - DirFilteredCloud.points.size() << " points are filtered out." << std::endl;
    return DirFilteredCloud;
}

void PipesSetCloud::CloudSegmentation(double tolerance) {
    if (SEGMENT_INPUT_CLOUD.points.size() == 0)
    {
        throw std::runtime_error("Input cloud is empty!");
    }
    PointCloud<PointXYZ> remaining_cloud = SEGMENT_INPUT_CLOUD;
    auto pInputCloud = std::make_shared<PointCloud<PointXYZ>>(remaining_cloud);

    // 创建KD树对象
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    tree->setInputCloud(pInputCloud);

    // 创建欧几里得聚类提取对象
    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
    ec.setClusterTolerance(tolerance); // 设置聚类容差
    ec.setMinClusterSize(SEGMENT_INPUT_CLOUD.size() / 50);    // 设置最小聚类点数
    ec.setMaxClusterSize(SEGMENT_INPUT_CLOUD.size());  // 设置最大聚类点数
    ec.setSearchMethod(tree);
    ec.setInputCloud(pInputCloud);
    ec.extract(cluster_indices);

    // 输出聚类结果
    int j = 0;
    for (const auto& cluster : cluster_indices) {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
        EndFaceCloud* pCurEndFaceCloud = new EndFaceCloud();
        pcl::Normal CurNormal;
        for (const auto& idx : cluster.indices) {
            pCurEndFaceCloud->push_back((*pInputCloud)[idx]);
            //pCurEndFaceCloud->normal_cloud.push_back((*pDirFilteredCloudNormals)[idx]);
        }
        pCurEndFaceCloud->width = cloud_cluster->size();
        pCurEndFaceCloud->height = 1;
        pCurEndFaceCloud->is_dense = true;
        //EndFaceCloud* pCurEndFaceCloud = new EndFaceCloud(*cloud_cluster);
        //for (const auto& idx : cluster.indices) {
        //    pCurEndFaceCloud->normal_cloud = *pDirFilteredCloudNormals[idx];
       // }
         
        this->pEndFaceClouds.push_back(pCurEndFaceCloud);
        //pCurEndFaceCloud->Display();
        std::cout << "Ring " << j << " has " << cloud_cluster->size() << " points." << std::endl;
        j++;
    }
    
}

PipesSetCloud::~PipesSetCloud() {
    if (this->pEndFaceClouds.size() > 0) {
        //遍历this->pEndFaceClouds
        for (auto iter = this->pEndFaceClouds.begin(); iter != this->pEndFaceClouds.end(); iter++) {
            EndFaceCloud* pCurEndFaceCloud = *iter;
            pCurEndFaceCloud->~EndFaceCloud();
            delete pCurEndFaceCloud;
            pCurEndFaceCloud = nullptr;
        }
        this->pEndFaceClouds.clear();
    }
}