//
// Created by zx on 2019/12/6.
//
#include "region_detect.h"

Region_detector::Region_detector(int id, EleFence::Region region):m_region_id(-1)
{
    m_region_param.CopyFrom(region);
    m_region_id = id;
    for (int i = 0; i < 4; ++i) {
        m_cloud_segs.push_back(pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
    }
}

Region_detector::~Region_detector()
{

}

bool Region_detector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in)
{
    if(!m_region_param.has_maxx() || !m_region_param.has_maxy() || !m_region_param.has_minx() || !m_region_param.has_miny())
        return false;
//    if(cloud_in->size() <= 0)
//        return false;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_copy(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::copyPointCloud(*cloud_in, *cloud_copy);
    if(cloud_copy->size() <= 0)
        return false;
    // 直通滤波, 筛选xy
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud(cloud_copy);
    pass.setFilterFieldName("x");//设置想在哪个坐标轴上操作
    pass.setFilterLimits(m_region_param.minx(), m_region_param.maxx());//将x轴的0到1范围内
    pass.setFilterLimitsNegative(false);//保留（true就是删除，false就是保留而删除此区间外的）
    pass.filter(*cloud_filtered);//输出到结果指针

    pass.setInputCloud(cloud_filtered);
    pass.setFilterFieldName("y");//设置想在哪个坐标轴上操作
    pass.setFilterLimits(m_region_param.miny(), m_region_param.maxy());//将x轴的0到1范围内
    pass.setFilterLimitsNegative(false);//保留（true就是删除，false就是保留而删除此区间外的）
    pass.filter(*cloud_filtered);//输出到结果指针

    if(cloud_filtered->size() <= 0)
        return false;
    // 离群点过滤
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
    sor.setInputCloud(cloud_filtered);
    sor.setMeanK(4); //K近邻搜索点个数
    sor.setStddevMulThresh(4.0); //标准差倍数
    sor.setNegative(false); //保留未滤波点（内点）
    sor.filter(*cloud_filtered);  //保存滤波结果到cloud_filter

    if(cloud_filtered->size() <= 0)
        return false;

    /////聚类
    pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
    kdtree->setInputCloud(cloud_filtered);

    pcl::EuclideanClusterExtraction<pcl::PointXYZ> clustering;
    // 设置聚类的最小值 2cm (small values may cause objects to be divided
    // in several clusters, whereas big values may join objects in a same cluster).
    clustering.setClusterTolerance(0.2);
    // 设置聚类的小点数和最大点云数
    clustering.setMinClusterSize(10);
    clustering.setMaxClusterSize(500);
    clustering.setSearchMethod(kdtree);
    clustering.setInputCloud(cloud_filtered);
    std::vector<pcl::PointIndices> clusters;
    clustering.extract(clusters);

    for (int i = 0; i < 4; ++i) {
        m_cloud_segs[i]->clear();
    }
    if(clusters.size()==4)
    {
        int j = 0;
        for (std::vector<pcl::PointIndices>::const_iterator it = clusters.begin(); it != clusters.end(); ++it)
        {
            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
            //创建新的点云数据集cloud_cluster，将所有当前聚类写入到点云数据集中
            for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
            {
                cloud_cluster->points.push_back(cloud_filtered->points[*pit]);
                cloud_cluster->width = cloud_cluster->points.size();
                cloud_cluster->height = 1;
                cloud_cluster->is_dense = true;
            }
            m_cloud_segs[j]=cloud_cluster;
            j++;
        }
//        std::cout<<" cluster class:"<<clusters.size()<<std::endl;
        return true;
    }
//    std::cout<< ros::Time::now()<<"    cluster failed   class: "<<clusters.size()<<std::endl;
    return false;
}