EuclidianExtractor.hpp

#ifndef CLOUDLIB_EUCLIDIANEXTRACTOR_HPP_
#define CLOUDLIB_EUCLIDIANEXTRACTOR_HPP_

#include <pcl/common/transforms.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/point_types.h>
#include <pcl/segmentation/extract_clusters.h>

#include "ClusterMesh.hpp"
#include "SmartCloud.hpp"

namespace ofxCloudLib
{

class EuclidianExtractor : public ofThread
{
  public:
    EuclidianExtractor()
    {
        parameters.setName("Euclidian Extraction");
        parameters.add(cluster_distance_tolerance.set("tolerance", .15, .01, 1));
        parameters.add(cluster_minimum_points.set("minimum points", 250, 1, 500));
        parameters.add(cluster_dither_amount.set("dither", 0.4, 0, 10));
        parameters.add(cluster_voxel_resolution.set("voxel resolution", .03, .01, .5));
        parameters.add(cluster_historical_threshold.set("cluster histo thresh", .3, .1, 1));
        parameters.add(cluster_halo.set("halo de tolérance", 0.2, 0, 2));
        startThread();
    };

    ~EuclidianExtractor()
    {
        extracted_.close();
        to_extract_.close();
        waitForThread(true);
    }

    void draw()
    {
        for (const auto &clustermesh : clusterMeshes) {
            clustermesh->draw();
        }
    }

    void toPCL(const ofMesh &mesh, pcl::PointCloud<pcl::PointXYZ>::Ptr pc)
    {
        pc->clear();
        for (unsigned int i = 0; i < mesh.getVertices().size(); i++) {
            const ofVec3f &v = mesh.getVertices()[i];
            pc->push_back(pcl::PointXYZ(v.x, v.y, v.z));
        }
    }

    double random_real()
    {
        return double(rand()) / double(RAND_MAX);
    }

    void threadedFunction()
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;

        while (to_extract_.receive(cloud)) {

//            if (cloud->size() > 10) {


  //              TS_START("EuclidianExtraction");

                std::vector<pcl::PointIndices> cluster_indices;
                pcl::PointCloud<pcl::PointXYZRGBA>::Ptr supercloudFilteredRGBA;

                pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
                tree->setInputCloud(cloud);

                pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
                ec.setClusterTolerance(cluster_distance_tolerance);
                ec.setMinClusterSize(cluster_minimum_points);
                ec.setMaxClusterSize(100000); // whatever? mettre 100k au lieu de 25k semble regler un crash rare
                ec.setSearchMethod(tree);
                ec.setInputCloud(cloud);
                ec.extract(cluster_indices);

                supercloudFilteredRGBA.reset(new pcl::PointCloud<pcl::PointXYZRGBA>);

                auto fresh_clouds = make_shared<vector<shared_ptr<SmartCloud>>>();

                for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
                    // nécessaire de copier, ou simplement passer des indices???
                    fresh_clouds->push_back(shared_ptr<SmartCloud>(new SmartCloud));
                    for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit) {
                        fresh_clouds->back().get()->cloud->points.push_back(cloud->points[*pit]);
                        pcl::PointXYZRGBA p;

                        double ditherX = (random_real() * (cluster_voxel_resolution * cluster_dither_amount)) - cluster_voxel_resolution * (cluster_dither_amount / 2);
                        double ditherY = (random_real() * (cluster_voxel_resolution * cluster_dither_amount)) - cluster_voxel_resolution * (cluster_dither_amount / 2);
                        double ditherZ = (random_real() * (cluster_voxel_resolution * cluster_dither_amount)) - cluster_voxel_resolution * (cluster_dither_amount / 2);
                        p.x            = cloud->points[*pit].x + ditherX;
                        p.y            = cloud->points[*pit].y + ditherY;
                        p.z            = cloud->points[*pit].z + ditherZ;
                        supercloudFilteredRGBA->points.push_back(p);
                    }
                    fresh_clouds->back().get()->setup();
                }

                vector<shared_ptr<ClusterMesh>>::iterator clusterMesh;
                vector<shared_ptr<SmartCloud>> candidateClouds = newClouds;
                vector<shared_ptr<SmartCloud>> nextCandidateClouds;

                for (clusterMesh = clusterMeshes.begin(); clusterMesh != clusterMeshes.end(); clusterMesh++) {

                    bool orphan = true;
                    vector<shared_ptr<SmartCloud>>::iterator newCloud;
                    for (newCloud = candidateClouds.begin(); newCloud != candidateClouds.end(); newCloud++) {

                        Eigen::Vector4f difference = (*clusterMesh)->smartCloud->centroid - (*newCloud)->centroid;
                        float d                    = sqrt(pow(difference[0], 2) + pow(difference[1], 2) + pow(difference[2], 2));
                        if (d < cluster_historical_threshold) {
                            //                std::cout << "maintain: d = " << d <<std::endl;
                            (*clusterMesh)->smartCloud = *newCloud;
                            (*clusterMesh)->setupMesh();
                            (*clusterMesh)->lostTime = 0;
                            orphan                   = false;

#define HALO .2
                        } else if ((*clusterMesh)->lost == true) {
                            if ((((*newCloud)->min[0] < (*clusterMesh)->max[0] + cluster_halo) && ((*newCloud)->max[0] > (*clusterMesh)->min[0] - cluster_halo))) {
                                // overlap X

                                if ((((*newCloud)->min[1] < (*clusterMesh)->max[1] + cluster_halo) && ((*newCloud)->max[1] > (*clusterMesh)->min[1] - cluster_halo))) {
                                    // overlap X+Y

                                    std::cout << "OVERLAP 1 " << std::endl;
                                    (*clusterMesh)->smartCloud = *newCloud;
                                    (*clusterMesh)->setupMesh();
                                    (*clusterMesh)->lostTime    = 0;
                                    (*clusterMesh)->activeSpree = 0;
                                    (*clusterMesh)->lost        = false;
                                    //                       newCloud = candidateClouds.erase(newCloud);
                                    orphan = false;
                                    //                        break;
                                }
                            }

                        } else {
                            nextCandidateClouds.push_back(*newCloud);
                        }
                    }
                    (*clusterMesh)->lost = orphan;
                    candidateClouds      = nextCandidateClouds;
                    nextCandidateClouds.clear();
                }

                /*
             3) pour chaque mesh restant:
             flag de la mort (fadeout)
             si flag < 0 = retrait du mesh
             */

                for (clusterMesh = clusterMeshes.begin(); clusterMesh != clusterMeshes.end();) {
                    (*clusterMesh)->age++;
                    if ((*clusterMesh)->lost == true) {


                        bool reborn = false;
                        vector<shared_ptr<SmartCloud>>::iterator c;
                        for (c = candidateClouds.begin(); c != candidateClouds.end(); c++) {

                            // test x

                            //                        if ((((*c)->min[0] < (*clusterMesh)->max[0]) && ((*c)->max[0] > (*clusterMesh)->min[0]))) {
                            //                            // overlap X
                            //                            if ((((*c)->min[1] < (*clusterMesh)->max[1]) && ((*c)->max[1] > (*clusterMesh)->min[1]))) {
                            //                                // overlap X+Y
                            if ((((*c)->min[0] < (*clusterMesh)->max[0] + cluster_halo) && ((*c)->max[0] > (*clusterMesh)->min[0] - cluster_halo))) {
                                // overlap X
                                if ((((*c)->min[1] < (*clusterMesh)->max[1] + cluster_halo) && ((*c)->max[1] > (*clusterMesh)->min[1] - cluster_halo))) {
                                    // overlap X+Y
                                    std::cout << "OVERLAP 2 avec tolérance" << cluster_halo << std::endl;
                                    (*clusterMesh)->smartCloud = *c;
                                    (*clusterMesh)->setupMesh();
                                    (*clusterMesh)->lostTime    = 0;
                                    (*clusterMesh)->lost        = false;
                                    (*clusterMesh)->activeSpree = 0;
                                    std::cout << "delete cloud " << std::endl;
                                    c      = candidateClouds.erase(c);
                                    reborn = true;
                                    break;
                                }
                            }
                        }

                        if (!reborn) {
                            (*clusterMesh)->lostTime++;
                            if ((*clusterMesh)->lostTime > 2) {
                                std::cout << "delete clustermesh " << std::endl;
                                clusterMeshBin.push_back(*clusterMesh);
                                clusterMesh = clusterMeshes.erase(clusterMesh);
                            }
                        }

                    } else {
                        (*clusterMesh)->activeSpree++;
                        ++clusterMesh;
                    }
                }

             /*
             
             2) pour chaque cloud non-associé:
             on regarde les mesh restant
             si un mesh est assez proche (seuil2)
             cloud = mesh -> associated = true
             sinon: newmesh!
             
             */

                vector<shared_ptr<SmartCloud>>::iterator newCloud;
                for (newCloud = candidateClouds.begin(); newCloud != candidateClouds.end(); newCloud++) {
                    std::cout << "new a clusterMesh" << std::endl;

                    if (clusterMeshBin.size() > 0) {
                        shared_ptr<ClusterMesh> revived = clusterMeshBin.back();
                        revived->smartCloud             = *newCloud;
                        revived->fresh                  = true;
                        revived->activeSpree            = 0;
                        revived->setupMesh();
                        clusterMeshes.push_back(revived);
                        clusterMeshBin.pop_back();

                    } else {
                        clusterMeshes.push_back(shared_ptr<ClusterMesh>(new ClusterMesh));
                        clusterMeshes.back().get()->smartCloud = *newCloud;
                        clusterMeshes.back().get()->setupFont();
                        clusterMeshes.back().get()->setupMesh(colors[clusterCounter % 12], clusterCounter);
                        clusterCounter++;
                    }
                }
                extracted_.send(fresh_clouds);
    //            TS_STOP("EuclidianExtraction");
 //           }
        }
    }

    void send(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
    {
        to_extract_.send(cloud);
    }

    bool receive()
    {
        if (extracted_.tryReceive(extracted_clouds_)) {
            return true;
        } else {
         //   ofLogNotice("EuclidianExtraction") << "nothing to receive";
        }
        return false;
    }

    shared_ptr<vector<shared_ptr<SmartCloud>>> get_clouds()
    {
        return extracted_clouds_;
    }

    vector<shared_ptr<SmartCloud>> *extract(const ofMesh &mesh)
    {

        ofLogNotice("Extractor") << "extract";

        cloud_.reset(new pcl::PointCloud<pcl::PointXYZ>);

        std::vector<pcl::PointIndices> cluster_indices;
        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr supercloudFilteredRGBA;

        toPCL(mesh, cloud_);

        pcl::VoxelGrid<pcl::PointXYZ> voxel_filter;
        voxel_filter.setInputCloud(cloud_);
        voxel_filter.setLeafSize(cluster_voxel_resolution, cluster_voxel_resolution, cluster_voxel_resolution);
        voxel_filter.filter(*cloud_);

        pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
        tree->setInputCloud(cloud_);

        pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
        ec.setClusterTolerance(cluster_distance_tolerance);
        ec.setMinClusterSize(cluster_minimum_points);
        ec.setMaxClusterSize(25000); // whatever?
        ec.setSearchMethod(tree);
        ec.setInputCloud(cloud_);
        ec.extract(cluster_indices);

        supercloudFilteredRGBA.reset(new pcl::PointCloud<pcl::PointXYZRGBA>);

        newClouds.clear();
        for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
            // nécessaire de copier, ou simplement passer des indices???
            newClouds.push_back(shared_ptr<SmartCloud>(new SmartCloud));
            for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit) {
                newClouds.back().get()->cloud->points.push_back(cloud_->points[*pit]);
                pcl::PointXYZRGBA p;

                double ditherX = (random_real() * (cluster_voxel_resolution * cluster_dither_amount)) - cluster_voxel_resolution * (cluster_dither_amount / 2);
                double ditherY = (random_real() * (cluster_voxel_resolution * cluster_dither_amount)) - cluster_voxel_resolution * (cluster_dither_amount / 2);
                double ditherZ = (random_real() * (cluster_voxel_resolution * cluster_dither_amount)) - cluster_voxel_resolution * (cluster_dither_amount / 2);
                p.x            = cloud_->points[*pit].x + ditherX;
                p.y            = cloud_->points[*pit].y + ditherY;
                p.z            = cloud_->points[*pit].z + ditherZ;
                supercloudFilteredRGBA->points.push_back(p);
            }
            newClouds.back().get()->setup();
        }

        vector<shared_ptr<ClusterMesh>>::iterator clusterMesh;
        vector<shared_ptr<SmartCloud>> candidateClouds = newClouds;
        vector<shared_ptr<SmartCloud>> nextCandidateClouds;

        for (clusterMesh = clusterMeshes.begin(); clusterMesh != clusterMeshes.end(); clusterMesh++) {

            bool orphan = true;
            vector<shared_ptr<SmartCloud>>::iterator newCloud;
            for (newCloud = candidateClouds.begin(); newCloud != candidateClouds.end(); newCloud++) {

                Eigen::Vector4f difference = (*clusterMesh)->smartCloud->centroid - (*newCloud)->centroid;
                float d                    = sqrt(pow(difference[0], 2) + pow(difference[1], 2) + pow(difference[2], 2));
                if (d < cluster_historical_threshold) {
                    //                std::cout << "maintain: d = " << d <<std::endl;
                    (*clusterMesh)->smartCloud = *newCloud;
                    (*clusterMesh)->setupMesh();
                    (*clusterMesh)->lostTime = 0;
                    orphan                   = false;

#define HALO .2
                } else if ((*clusterMesh)->lost == true) {
                    if ((((*newCloud)->min[0] < (*clusterMesh)->max[0] + cluster_halo) && ((*newCloud)->max[0] > (*clusterMesh)->min[0] - cluster_halo))) {
                        // overlap X

                        if ((((*newCloud)->min[1] < (*clusterMesh)->max[1] + cluster_halo) && ((*newCloud)->max[1] > (*clusterMesh)->min[1] - cluster_halo))) {
                            // overlap X+Y

                            std::cout << "OVERLAP 1 " << std::endl;
                            (*clusterMesh)->smartCloud = *newCloud;
                            (*clusterMesh)->setupMesh();
                            (*clusterMesh)->lostTime    = 0;
                            (*clusterMesh)->activeSpree = 0;
                            (*clusterMesh)->lost        = false;
                            //                       newCloud = candidateClouds.erase(newCloud);
                            orphan = false;
                            //                        break;
                        }
                    }

                } else {
                    nextCandidateClouds.push_back(*newCloud);
                }
            }
            (*clusterMesh)->lost = orphan;
            candidateClouds      = nextCandidateClouds;
            nextCandidateClouds.clear();
        }

        /*
             3) pour chaque mesh restant:
             flag de la mort (fadeout)
             si flag < 0 = retrait du mesh
             */

        for (clusterMesh = clusterMeshes.begin(); clusterMesh != clusterMeshes.end();) {
            (*clusterMesh)->age++;
            if ((*clusterMesh)->lost == true) {


                bool reborn = false;
                vector<shared_ptr<SmartCloud>>::iterator c;
                for (c = candidateClouds.begin(); c != candidateClouds.end(); c++) {

                    // test x

                    //                        if ((((*c)->min[0] < (*clusterMesh)->max[0]) && ((*c)->max[0] > (*clusterMesh)->min[0]))) {
                    //                            // overlap X
                    //                            if ((((*c)->min[1] < (*clusterMesh)->max[1]) && ((*c)->max[1] > (*clusterMesh)->min[1]))) {
                    //                                // overlap X+Y
                    if ((((*c)->min[0] < (*clusterMesh)->max[0] + cluster_halo) && ((*c)->max[0] > (*clusterMesh)->min[0] - cluster_halo))) {
                        // overlap X
                        if ((((*c)->min[1] < (*clusterMesh)->max[1] + cluster_halo) && ((*c)->max[1] > (*clusterMesh)->min[1] - cluster_halo))) {
                            // overlap X+Y
                            std::cout << "OVERLAP 2 avec tolérance" << cluster_halo << std::endl;
                            (*clusterMesh)->smartCloud = *c;
                            (*clusterMesh)->setupMesh();
                            (*clusterMesh)->lostTime    = 0;
                            (*clusterMesh)->lost        = false;
                            (*clusterMesh)->activeSpree = 0;
                            std::cout << "delete cloud " << std::endl;
                            c      = candidateClouds.erase(c);
                            reborn = true;
                            break;
                        }
                    }
                }

                if (!reborn) {
                    (*clusterMesh)->lostTime++;
                    if ((*clusterMesh)->lostTime > 2) {
                        std::cout << "delete clustermesh " << std::endl;
                        clusterMeshBin.push_back(*clusterMesh);
                        clusterMesh = clusterMeshes.erase(clusterMesh);
                    }
                }

            } else {
                (*clusterMesh)->activeSpree++;
                ++clusterMesh;
            }
        }

        /*
             
             2) pour chaque cloud non-associé:
             on regarde les mesh restant
             si un mesh est assez proche (seuil2)
             cloud = mesh -> associated = true
             sinon: newmesh!
             
             */

        vector<shared_ptr<SmartCloud>>::iterator newCloud;
        for (newCloud = candidateClouds.begin(); newCloud != candidateClouds.end(); newCloud++) {
            std::cout << "new a clusterMesh" << std::endl;

            if (clusterMeshBin.size() > 0) {
                shared_ptr<ClusterMesh> revived = clusterMeshBin.back();
                revived->smartCloud             = *newCloud;
                revived->fresh                  = true;
                revived->activeSpree            = 0;
                revived->setupMesh();
                clusterMeshes.push_back(revived);
                clusterMeshBin.pop_back();

            } else {
                clusterMeshes.push_back(shared_ptr<ClusterMesh>(new ClusterMesh));
                clusterMeshes.back().get()->smartCloud = *newCloud;
                clusterMeshes.back().get()->setupFont();
                clusterMeshes.back().get()->setupMesh(colors[clusterCounter % 12], clusterCounter);
                clusterCounter++;
            }
        }
        return &newClouds;
    }

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_;
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr supercloudFilteredRGBA;

    std::vector<pcl::PointIndices> cluster_indices;
    vector<shared_ptr<SmartCloud>> newClouds;

    vector<shared_ptr<ClusterMesh>> clusterMeshes;
    deque<shared_ptr<ClusterMesh>> clusterMeshBin;

    ofThreadChannel<pcl::PointCloud<pcl::PointXYZ>::Ptr> to_extract_;
    ofThreadChannel<shared_ptr<vector<shared_ptr<SmartCloud>>>> extracted_;

    shared_ptr<vector<shared_ptr<SmartCloud>>> extracted_clouds_;

    ofParameterGroup parameters;
    ofParameter<float> cluster_distance_tolerance;
    ofParameter<float> cluster_minimum_points;
    ofParameter<float> cluster_dither_amount;
    ofParameter<float> cluster_voxel_resolution;
    ofParameter<float> cluster_historical_threshold;
    ofParameter<float> cluster_halo;

    int clusterCounter = 0;

    vector<ofFloatColor> colors = {
        ofFloatColor(1, 1, 0, 1),
        ofFloatColor(1, 0, 1, 1),
        ofFloatColor(0, 1, 1, 1),
        ofFloatColor(1, 0, 0, 1),
        ofFloatColor(0, 1, 0, 1),
        ofFloatColor(.1, .1, 1, 1),
        ofFloatColor(1, .5, 0, 1),
        ofFloatColor(.5, 1, 0, 1),
        ofFloatColor(0, .5, 1, 1),
        ofFloatColor(0, 1, .5, 1),
        ofFloatColor(.5, 0, 255, 1),
        ofFloatColor(1, 0, .5, 1)};
};
}

#endif // CLOUDLIB_EUCLIDIANEXTRACTOR_HPP_