/* 
 * File:   PclPointCloud.cpp
 * Author: carlos
 * 
 * Created on 26 de octubre de 2012, 15:45
 */

#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET


#include <PclPointCloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/surface/mls.h>
#include <pcl/surface/gp3.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/3dsc.h>
#include <pcl/features/principal_curvatures.h>
#include <pcl/features/feature.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/common/transforms.h>
#include <pcl/common/angles.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_polygonal_prism_data.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <operations.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/common/impl/centroid.hpp>

#include <CpuTime.h>

PclPointCloud::PclPointCloud() {
    m_stopVisualizer = false;
    for (int i = 0; i < NUM_CLOUDS; i++) {
        m_cloud[i] = pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
        m_cloudNormal[i] = pcl::PointCloud<pcl::Normal>::Ptr(new pcl::PointCloud<pcl::Normal>);
    }

}

PclPointCloud::PclPointCloud(const PclPointCloud& orig) {
}

PclPointCloud::~PclPointCloud() {
}

int PclPointCloud::startPclVisualizerThread(string windowName) {
    m_visualizerTh = boost::thread(boost::bind(&PclPointCloud::initPclVisualizer, this, windowName));
    return 0;
}

int PclPointCloud::addPlaneToView(pcl::ModelCoefficients coeff, string name) {
    return m_viewer->addPlane(coeff, name);
}

int PclPointCloud::removePlaneFromView(string name) {
    return m_viewer->removeShape(name);
}

int PclPointCloud::removeAllObjects() {
    return m_viewer->removeAllShapes(0);
}

double PclPointCloud::distPtPlaneSigned(pcl::PointXYZRGB pt, pcl::ModelCoefficients coeff) {

    double denom = sqrt((coeff.values.at(0) * coeff.values.at(0))+(coeff.values.at(1) * coeff.values.at(1))+(coeff.values.at(2) * coeff.values.at(2)));
    double num = coeff.values.at(0) * pt.x + coeff.values.at(1) * pt.y + coeff.values.at(2) * pt.z + coeff.values.at(3);
    //double num = fabs(coeff.values.at(0) * pt.x + coeff.values.at(1) * pt.y + coeff.values.at(2) * pt.z + coeff.values.at(3));
    return num / denom;
}

double PclPointCloud::distPtPlane(pcl::PointXYZRGB pt, pcl::ModelCoefficients coeff) {

    double denom = sqrt((coeff.values.at(0) * coeff.values.at(0))+(coeff.values.at(1) * coeff.values.at(1))+(coeff.values.at(2) * coeff.values.at(2)));
    //double num = coeff.values.at(0) * pt.x + coeff.values.at(1) * pt.y + coeff.values.at(2) * pt.z + coeff.values.at(3);
    double num = fabs(coeff.values.at(0) * pt.x + coeff.values.at(1) * pt.y + coeff.values.at(2) * pt.z + coeff.values.at(3));
    return num / denom;
}

void PclPointCloud::setPointColor(int idCloud, int idxPoint, int r, int g, int b) {
    m_cloud[idCloud]->points.at(idxPoint).r = r;
    m_cloud[idCloud]->points.at(idxPoint).g = g;
    m_cloud[idCloud]->points.at(idxPoint).b = b;
}

int PclPointCloud::calcCentroid(int idCloudIn, pcl::PointIndices indicesIn, Eigen::Vector4f* centroid) {
    int ret = 0;

    pcl::compute3DCentroid<pcl::PointXYZRGB>(*m_cloud[idCloudIn], indicesIn, *centroid);

    return ret;
}

int PclPointCloud::modePolygonArea(int idCloud, int idCloudPolygon, pcl::ModelCoefficients coeff, float beanSize, float* value, int* n) {
    double z_min = -20.0, z_max = 20.0;
    int ret = 0;

    CpuTime cpu1;


    //get points in polygon
    cpu1.start();
    pcl::PointIndices cloud_indices;
    pcl::ExtractPolygonalPrismData<pcl::PointXYZRGB> prism;
    prism.setInputCloud(m_cloud[idCloud]);
    prism.setInputPlanarHull(m_cloud[idCloudPolygon]);
    prism.setHeightLimits(z_min, z_max);
    prism.segment(cloud_indices);
    cpu1.stop();
    printf("\tsegmented %d points of %d in polygon of %d vertex = %s\n",
           cloud_indices.indices.size(), m_cloud[idCloud]->points.size(),
           m_cloud[idCloudPolygon]->size(), cpu1.getText().data());



    //    cpu1.start();
    vector<int> indices = cloud_indices.indices;
    cv::Mat dataHist(1, indices.size(), CV_32FC1);

    //fill input data to the histogram function with the point distance to plane
    double sum = 0;
    for (int i = 0; i < indices.size(); i++) {
        dataHist.at<float>(0, i) = distPtPlaneSigned(m_cloud[idCloud]->points.at(indices.at(i)), coeff);
        sum += dataHist.at<float>(0, i);
    }
    //    cpu1.stop();
    //    printf("\tDistance points to plane = %s\n",cpu1.getText().data());

    //    cpu1.start();
    float zMode;
    cv::Mat histo;
    try {
        double z_min, z_max;
        cv::minMaxLoc(dataHist, &z_min, &z_max, NULL, NULL);
        *(n) = histogram(dataHist, z_min, z_max, beanSize, &zMode, cv::Mat(), &histo);

        //option 1: mode        
        //        *(value) = zMode;

        //option 2: mean
        *(value) = sum / indices.size(); //test using mean instead of histogram

        //option 3: gaussian
        //        float peakG, meanG, varG;
        //        gaussianFit(z_min, histo, beanSize, &peakG, &meanG, &varG);
        //        *(n) = (int) peakG;
        //        *(value) = (int) meanG;
    }
    catch (cv::Exception exc){
        *(n) = 0;
        *(value) = 0.0;
    }
    //    cpu1.stop();
    //    printf("\tHistogram = %s\n",cpu1.getText().data());

    //    printf("zHist = %.3f\n", *value);

    return 0;
}

int PclPointCloud::modePolygonAreaEfficient(int idCloud, vector<int> ptInlierIndex, pcl::ModelCoefficients coeff, float beanSize, float* value, int* n) {
    double z_min = -20.0, z_max = 20.0;
    int ret = 0;

    CpuTime cpu1;

    //    cpu1.start();
    vector<int> indices = ptInlierIndex;
    cv::Mat dataHist(1, indices.size(), CV_32FC1);

    //fill input data to the histogram function with the point distance to plane
    double sum = 0;
    for (int i = 0; i < indices.size(); i++) {
        dataHist.at<float>(0, i) = distPtPlaneSigned(m_cloud[idCloud]->points.at(indices.at(i)), coeff);
        sum += dataHist.at<float>(0, i);
    }
    //    cpu1.stop();
    //    printf("\tDistance points to plane = %s\n",cpu1.getText().data());

    //    cpu1.start();
    float zMode;
    cv::Mat histo;
    try {
        double z_min, z_max;
        cv::minMaxLoc(dataHist, &z_min, &z_max, NULL, NULL);
        *(n) = histogram(dataHist, z_min, z_max, beanSize, &zMode, cv::Mat(), &histo);

        //option 1: mode        
        //        *(value) = zMode;

        //option 2: mean
        *(value) = sum / indices.size(); //test using mean instead of histogram

        //option 3: gaussian
        //        float peakG, meanG, varG;
        //        gaussianFit(z_min, histo, beanSize, &peakG, &meanG, &varG);
        //        *(n) = (int) peakG;
        //        *(value) = (int) meanG;
    }
    catch (cv::Exception exc){
        *(n) = 0;
        *(value) = 0.0;
    }
    //    cpu1.stop();
    //    printf("\tHistogram = %s\n",cpu1.getText().data());

    //    printf("zHist = %.3f\n", *value);

    return 0;
}

int PclPointCloud::showPointCloud(int idCloud) {


    //    m_viewer->removePolygonMesh("surface");
    m_viewer->updatePointCloud(m_cloud[idCloud], string("original"));
    printf("showPointCloud.%d points\n", m_cloud[idCloud]->points.size());
    //    if (idCloud==this->triangulation){
    //        m_viewer->addPolygonMesh(m_triangles,"surface");
    //    }
    return 0;
}

int PclPointCloud::computeNormals(int idCloud, int kSearch) {

    // Normal estimation    
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);

    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudxyz = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::copyPointCloud<pcl::PointXYZRGB, pcl::PointXYZ>(*(m_cloud[idCloud]), *cloudxyz);
    tree->setInputCloud(cloudxyz);

    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
    n.setInputCloud(cloudxyz);
    n.setSearchMethod(tree);
    n.setKSearch(kSearch);
    n.compute(*normals);

    *(m_cloudNormal[idCloud]) = *normals;

    return 0;
}

int PclPointCloud::computeRegionNormals(int idCloud, double radius, float x, float y, float z) {

    const int MAX_NN = 0;
    const float RAD_NORM = 0.25;
    CpuTime cpu1;
    cpu1.start();

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudxyz = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::copyPointCloud<pcl::PointXYZRGB, pcl::PointXYZ>(*(m_cloud[idCloud]), *cloudxyz);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);

    pcl::PointXYZ searchPoint;
    searchPoint.x = x;
    searchPoint.y = y;
    searchPoint.z = z;

    std::vector<int> pointIdxRadiusSearch;
    std::vector<float> pointRadiusSquaredDistance;
    tree->setInputCloud(cloudxyz);
    tree->radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance, MAX_NN);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr xyzrgb = pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::copyPointCloud<pcl::PointXYZ, pcl::PointXYZRGB>(*cloudxyz, pointIdxRadiusSearch, *xyzrgb);

    printf("%d neighbors found\n", pointIdxRadiusSearch.size());
    cpu1.stop();
    printf("NN Computation = %s\n", cpu1.getText().data());

    for (int i = 0; i < xyzrgb->points.size(); i++) {
        xyzrgb->points.at(i).r = 0;
        xyzrgb->points.at(i).g = 255;
        xyzrgb->points.at(i).b = 0;
    }


    cpu1.start();
    // Normal estimation         
    pcl::PointIndicesPtr ind(new pcl::PointIndices);
    ind->indices = pointIdxRadiusSearch;
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
    n.setInputCloud(cloudxyz);
    n.setSearchMethod(tree);
    n.setRadiusSearch(RAD_NORM);
    n.setIndices(ind);
    n.compute(*normals);
    //    *(m_cloudNormal[idCloud]) = *normals;
    
    //curvatures
    pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr curvat = pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr(new pcl::PointCloud<pcl::PrincipalCurvatures>);
    pcl::PrincipalCurvaturesEstimation<pcl::PointXYZRGB, pcl::Normal, pcl::PrincipalCurvatures> pce;
    pce.setInputCloud(xyzrgb);
    pce.setInputNormals(normals);
    pce.setRadiusSearch(RAD_NORM);
//    pce.setIndices(ind);
    pce.compute(*curvat);
    
    //esto es solamente para representacion de los vectores en el visor
    for (int i=0; i<normals->points.size(); i++){
        normals->points.at(i).normal_x = fabs(curvat->points.at(i).principal_curvature_x);
        normals->points.at(i).normal_y = fabs(curvat->points.at(i).principal_curvature_y);
        normals->points.at(i).normal_z = fabs(curvat->points.at(i).principal_curvature_z);
    }
    
    pcl::copyPointCloud<pcl::PointXYZ, pcl::PointXYZ>(*cloudxyz, pointIdxRadiusSearch, *cloudxyz);    
    printf("%d normals\n", normals->points.size());
    cpu1.stop();
    printf("Normal Computation of region = %s\n", cpu1.getText().data());

    int linesRatio = 5;
    float linesLength = 0.05;
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(xyzrgb);

    //    if (!m_viewer->addPointCloudNormals<pcl::PointXYZRGB, pcl::Normal> (xyzrgb, normals, linesRatio, linesLength, string("normals"))) {
    //    m_viewer->removeAllPointClouds(0);
    m_viewer->removePointCloud(string("normals"));
    m_viewer->removePointCloud(string("xyznormals"));
    m_viewer->removePointCloud(string("curvatures"));
        m_viewer->addPointCloudNormals<pcl::PointXYZRGB, pcl::Normal> (xyzrgb, normals, linesRatio, linesLength, string("normals"));
//    m_viewer->addPointCloudPrincipalCurvatures(cloudxyz, normals, curvat, linesRatio, linesLength, string("curvatures"));
    m_viewer->addPointCloud<pcl::PointXYZRGB> (xyzrgb, rgb, string("xyznormals"));
    //    }
    //    else{
    //        m_viewer->addPointCloud<pcl::PointXYZRGB> (xyzrgb, rgb, string("xyznormals"));
    //    }




    return 0;
}

int PclPointCloud::computeNormals(int idCloud, double radius) {

    // Normal estimation    
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);

    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudxyz = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::copyPointCloud<pcl::PointXYZRGB, pcl::PointXYZ>(*(m_cloud[idCloud]), *cloudxyz);
    tree->setInputCloud(cloudxyz);

    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
    n.setInputCloud(cloudxyz);
    n.setSearchMethod(tree);
    //n.setKSearch(20);
    n.setRadiusSearch(radius);
    n.compute(*normals);

    *(m_cloudNormal[idCloud]) = *normals;
    printf("%d normals\n", normals->points.size());

    return 0;
}

void PclPointCloud::initPclVisualizer(string windowName) {

    if (m_viewer == NULL) {
        m_viewer = simpleVis(m_cloud[this->original], windowName);

        while (!m_stopVisualizer) {

            m_viewer->spinOnce(100);
            //        m_viewer->updatePointCloud(m_cloud[this->original],string("original")); 
            //        boost::this_thread::sleep (boost::posix_time::milliseconds (250));
        }
        printf("3D Viewer Closed\n");
        //    m_viewer->close();
    }

}

int PclPointCloud::setCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, int cloudId) {

    *(m_cloud[cloudId]) = *(cloud);
    return 0;
}

int PclPointCloud::getCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, int cloudId) {

    *(cloud) = *(m_cloud[cloudId]);
    return 0;
}

int PclPointCloud::getCurvaturesCloud(pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr cloud) {

    *(cloud) = *(m_curvaturesCloud);
    return 0;
}

pcl::PointXYZRGB PclPointCloud::getPoint(int idCloud, int idxPoint) {
    return m_cloud[idCloud]->points.at(idxPoint);
}

int PclPointCloud::detectOnePlane(int cloudIdIn, pcl::PointIndices indices, pcl::PointIndices *inliers, double threshold, pcl::ModelCoefficients *coeff) {
    int ret = 0;

    inliers->indices.clear();
    pcl::SACSegmentation<pcl::PointXYZRGB> sacSeg;
    sacSeg.setModelType(pcl::SACMODEL_PLANE);
    sacSeg.setMethodType(pcl::SAC_RANSAC);
    sacSeg.setDistanceThreshold(threshold);
    sacSeg.setOptimizeCoefficients(true);
    sacSeg.setInputCloud(m_cloud[cloudIdIn]);
    if (indices.indices.size() > 0) {
        pcl::PointIndicesPtr ind(new pcl::PointIndices);
        ind->indices = indices.indices;
        sacSeg.setIndices(ind);
    }
    pcl::ModelCoefficients coeff2;
    sacSeg.segment(*inliers, coeff2);
    *coeff = coeff2;

    return ret;
}

int PclPointCloud::removeIndices(int idCloud, pcl::PointIndices indicesRem, pcl::PointIndices* indicesOut, int idCloudOut) {
    int ret = 0;

    //pcl::PointIndicesConstPtr ptrInd = indicesRem.ConstPtr;
    pcl::PointIndicesPtr ptrInd(new pcl::PointIndices);
    ptrInd->indices = indicesRem.indices;
    pcl::ExtractIndices<pcl::PointXYZRGB> eifilter(true);
    eifilter.setInputCloud(m_cloud[idCloud]);
    eifilter.setIndices(ptrInd);
    eifilter.setNegative(true);
    eifilter.filter(indicesOut->indices);
    pcl::copyPointCloud(*m_cloud[idCloud], indicesOut->indices, *m_cloud[idCloudOut]);

    return 0;
}

void PclPointCloud::getPlaneDegreeInclination(pcl::ModelCoefficients plane, double* x, double* y, double* z) {

    *x = pcl::rad2deg(atan(plane.values.at(0)));
    *y = pcl::rad2deg(atan(plane.values.at(1)));
    *z = pcl::rad2deg(atan(plane.values.at(2)));
}

void PclPointCloud::getPlaneRadiansInclination(pcl::ModelCoefficients plane, double* x, double* y, double* z) {

    *x = atan(plane.values.at(0));
    *y = atan(plane.values.at(1));
    *z = atan(plane.values.at(2));
}

int PclPointCloud::rotTransCloud(int idCloudIn, int idCloudOut, double pitch, double roll, double yaw, double x, double y, double z) {
    int ret = 0;

    //change coordinates criteria from Sergio to PCL
    double sinRoll = sin(-yaw);
    double sinPitch = sin(roll);
    double sinYaw = sin(-pitch);
    double cosRoll = cos(-yaw);
    double cosPitch = cos(roll);
    double cosYaw = cos(-pitch);

    float r11 = cosRoll * cosYaw + sinRoll * sinPitch*sinYaw;
    float r12 = sinRoll*cosPitch;
    float r13 = -cosRoll * sinYaw + sinRoll * sinPitch*cosYaw;
    float r21 = -sinRoll * cosYaw + cosRoll * sinPitch*sinYaw;
    float r22 = cosRoll*cosPitch;
    float r23 = sinRoll * sinYaw + cosRoll * sinPitch*cosYaw;
    float r31 = cosPitch*sinYaw;
    float r32 = -sinPitch;
    float r33 = cosPitch*cosYaw;

    Eigen::Matrix4f rtMat;
    rtMat << r11, r12, r13, x,
            r21, r22, r23, y,
            r31, r32, r33, z,
            0.0, 0.0, 0.0, 1.0;

    pcl::transformPointCloud(*(m_cloud[idCloudIn]), *(m_cloud[idCloudOut]), rtMat);

    return ret;
}

int PclPointCloud::readCloudFromFile(string fileName, int cloudId) {

    if (pcl::io::loadPCDFile<pcl::PointXYZRGB> (fileName, *(m_cloud[cloudId])) == -1) {
        printf("Error Reading File: %s\n", fileName.data());
        return -1;
    }
    printf("cloud read. %d points\n", m_cloud[cloudId]->points.size());
    //    for(int i=0; i<m_cloud[cloudId]->points.size(); i++){
    //        printf("%d %f %f %f\n",i,m_cloud[cloudId]->points.at(i).x,m_cloud[cloudId]->points.at(i).y,m_cloud[cloudId]->points.at(i).z);
    //    }
    return 0;
}

void PclPointCloud::removeObstacles(int idCloudIn, int idCloudOut, double minX, double maxX, double minY, double maxY, pcl::PointIndices *indObst, bool color) {
    indObst->indices.clear();
    m_cloud[idCloudOut]->points.clear();
    for (int i = 0; i < m_cloud[idCloudIn]->points.size(); i++) {
        double currX = fabs(m_cloudNormal[idCloudIn]->points.at(i).normal_x);
        double currY = fabs(m_cloudNormal[idCloudIn]->points.at(i).normal_y);
        if ((currX >= minX && currX <= maxX)) {
            indObst->indices.push_back(i);
            if (color) {
                m_cloud[idCloudIn]->points.at(i).r = 0;
                m_cloud[idCloudIn]->points.at(i).g = 0;
                m_cloud[idCloudIn]->points.at(i).b = 128;
            }
        }
        else if ((currY >= minY && currY <= maxY)) {
            indObst->indices.push_back(i);
            if (color) {
                m_cloud[idCloudIn]->points.at(i).r = 0;
                m_cloud[idCloudIn]->points.at(i).g = 0;
                m_cloud[idCloudIn]->points.at(i).b = 128;
            }
        }
        else {
            m_cloud[idCloudOut]->points.push_back(m_cloud[idCloudIn]->points.at(i));
            //indObst->indices.push_back(i);
        }
    }
}

int PclPointCloud::readCloudNormalFromFile(string fileName, int cloudId) {

    if (pcl::io::loadPCDFile<pcl::Normal> (fileName, *(m_cloudNormal[cloudId])) == -1) {
        printf("Error Reading File: %s\n", fileName.data());
        return -1;
    }
    return 0;
}

int PclPointCloud::readCloudFromFile(string fileName, pcl::PointCloud<pcl::ShapeContext1980>::Ptr cloud) {

    pcl::ShapeContext1980 pt;
    const int size = 1989;
    float data[size];

    FILE *fd = fopen(fileName.data(), "rb");
    cloud->points.clear();

    while (fread(data, sizeof (float), size, fd) == size) {
        for (int i = 0; i < 1980; i++) {
            pt.descriptor[i]=data[i];
        }
        for (int i = 0; i < 9; i++) {
            pt.rf[i] = data[1980 + i];
        }
        cloud->points.push_back(pt);
//        pt.descriptor.clear();
    }
    fclose(fd);
    printf("cloud shape = %d points\n", cloud->points.size());

    return 0;
}

int PclPointCloud::writeCloudToFile(string fileName, pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud) {

    try {
        if (pcl::io::savePCDFileBinary(fileName, *(cloud)) == -1) {
            printf("Error Writing File: %s\n", fileName.data());
            return -1;
        }
        return 0;
    }
    catch (pcl::IOException exc){
        printf("Error Writing File: %s\n", fileName.data());
        printf("IOException: %s\n", exc.detailedMessage().data());
        return -1;
    }
}

int PclPointCloud::writeCloudToFile(string fileName, pcl::PointCloud<pcl::ShapeContext1980>::Ptr cloud) {

    FILE *fd = fopen(fileName.data(), "wb");
    int count = cloud->points.size();
    size_t written = 0;

    for (int i = 0; i < count; i++) {
        pcl::ShapeContext1980 pt = cloud->points.at(i);
        float elem;
        for (int i = 0; i < 1980; i++) {
            elem = pt.descriptor[i];
            written += fwrite(&elem, 1, sizeof (float), fd);
        }
        for (int i = 0; i < 9; i++) {
            elem = pt.rf[i];
            written += fwrite(&elem, 1, sizeof (float), fd);
        }

        //        written += fwrite(&pt.descriptor , 1980 , sizeof(float) , fd);
        //        written += fwrite (&pt.descriptor , 1 , sizeof(pt.descriptor) , fd);
        //        written += fwrite (&pt.rf , 9 , sizeof(float) , fd);
    }
    fclose(fd);
    //    printf("written = %ld (%d points)\n",written,count);

    return 0;

}

int PclPointCloud::writeCloudToFileTxt(string fileName, pcl::PointCloud<pcl::ShapeContext1980>::Ptr cloud) {

    FILE *fd = fopen(fileName.data(), "w");
    int count = cloud->points.size();

    for (int i = 0; i < count; i++) {
        pcl::ShapeContext1980 pt = cloud->points.at(i);
        float elem;
        for (int i = 0; i < 1980; i++) {
            elem = pt.descriptor[i];
            fprintf(fd, "%f ", elem);
        }
        for (int i = 0; i < 9; i++) {
            elem = pt.rf[i];
            fprintf(fd, "%f ", elem);
        }
        fprintf(fd, "\n");
    }
    fclose(fd);
    //    printf("written = %ld (%d points)\n",written,count);

    return 0;

}

int PclPointCloud::writeCloudToFile(string fileName, int cloudId) {

    try {
        if (pcl::io::savePCDFileBinary(fileName, *(m_cloud[cloudId])) == -1) {
            printf("Error Writing File: %s\n", fileName.data());
            return -1;
        }
        printf("saved: %s\n", fileName.data());
        return 0;
    }
    catch (pcl::IOException exc){
        printf("Error Writing File: %s\n", fileName.data());
        printf("IOException: %s\n", exc.detailedMessage().data());
        return -1;
    }
}

int PclPointCloud::writeCloudNormalToFile(string fileName, int cloudId) {

    try {
        if (pcl::io::savePCDFileBinary(fileName, *(m_cloudNormal[cloudId])) == -1) {
            printf("Error Writing File: %s\n", fileName.data());
            return -1;
        }
        return 0;
    }
    catch (pcl::IOException exc){
        printf("Error Writing File: %s\n", fileName.data());
        printf("IOException: %s\n", exc.detailedMessage().data());
        return -1;
    }
}

boost::shared_ptr<pcl::visualization::PCLVisualizer> PclPointCloud::simpleVis(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud, string windowName) {

    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer(windowName));
    viewer->setBackgroundColor(0, 0, 0);
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
    //0.106589,106.589/0,0,0/-5.05631,0.39072,7.26235/0.818448,-0.0491283,0.572477/0.523599/1274,512/3,23
    printf("simpleVis = %d pts\n", cloud->points.size());
    viewer->addPointCloud<pcl::PointXYZRGB> (cloud, rgb, "original");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original");
    viewer->addCoordinateSystem(1.0, 0, 0, 0);
    viewer->setCameraPosition(
                          -10, 0, 5, /*camera pose*/
                          8, 0, 0, /*camera view*/
                          3, 0, 0, /*up XYZ*/
                          0); /*viewport (all)*/

    return (viewer);
}

int PclPointCloud::conditionalRemoval(double xmin, double xmax, double ymin, double ymax, double zmin, double zmax, int inCloudId /* = 0 */, pcl::PointIndices indIn, int outCloudId /* = 0 */) {


    printf("\nConditional Removal\ninput = %d pts\n", m_cloud[inCloudId]->points.size());

    pcl::ConditionAnd<pcl::PointXYZRGB>::Ptr range_cond(new pcl::ConditionAnd<pcl::PointXYZRGB> ());
    range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(new
                                                                               pcl::FieldComparison<pcl::PointXYZRGB> ("z", pcl::ComparisonOps::GT, zmin)));
    range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(new
                                                                               pcl::FieldComparison<pcl::PointXYZRGB> ("z", pcl::ComparisonOps::LT, zmax)));
    range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(new
                                                                               pcl::FieldComparison<pcl::PointXYZRGB> ("x", pcl::ComparisonOps::GT, xmin)));
    range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(new
                                                                               pcl::FieldComparison<pcl::PointXYZRGB> ("x", pcl::ComparisonOps::LT, xmax)));
    range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(new
                                                                               pcl::FieldComparison<pcl::PointXYZRGB> ("y", pcl::ComparisonOps::GT, ymin)));
    range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(new
                                                                               pcl::FieldComparison<pcl::PointXYZRGB> ("y", pcl::ComparisonOps::LT, ymax)));

    // build the filter
    pcl::ConditionalRemoval<pcl::PointXYZRGB> condrem(range_cond);
    condrem.setInputCloud(m_cloud[inCloudId]);
    if (indIn.indices.size() > 0) {
        pcl::PointIndicesPtr indPtr(new pcl::PointIndices);
        indPtr->indices = indIn.indices;
        condrem.setIndices(indPtr);
    }
    condrem.filter(*m_cloud[outCloudId]);

    printf("output = %d pts\n", m_cloud[outCloudId]->points.size());

    return 0;
}

int PclPointCloud::conditionalRemoval(double xmin, double xmax, double ymin, double ymax, double zmin, double zmax, int inCloudId /* = 0 */, pcl::PointIndices indIn, pcl::PointIndices *indOut /* = 0 */) {


    //  printf("\nConditional Removal\ninput = %d pts\n", m_cloud[inCloudId]->points.size());

    pcl::ConditionAnd<pcl::PointXYZRGB>::Ptr range_cond(new pcl::ConditionAnd<pcl::PointXYZRGB> ());
    range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(new
                                                                               pcl::FieldComparison<pcl::PointXYZRGB> ("z", pcl::ComparisonOps::GT, zmin)));
    range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(new
                                                                               pcl::FieldComparison<pcl::PointXYZRGB> ("z", pcl::ComparisonOps::LT, zmax)));
    range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(new
                                                                               pcl::FieldComparison<pcl::PointXYZRGB> ("x", pcl::ComparisonOps::GT, xmin)));
    range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(new
                                                                               pcl::FieldComparison<pcl::PointXYZRGB> ("x", pcl::ComparisonOps::LT, xmax)));
    range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(new
                                                                               pcl::FieldComparison<pcl::PointXYZRGB> ("y", pcl::ComparisonOps::GT, ymin)));
    range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(new
                                                                               pcl::FieldComparison<pcl::PointXYZRGB> ("y", pcl::ComparisonOps::LT, ymax)));

    // build the filter
    pcl::ConditionalRemoval<pcl::PointXYZRGB> condrem(range_cond, true);
    condrem.setInputCloud(m_cloud[inCloudId]);
    if (indIn.indices.size() > 0) {
        pcl::PointIndicesPtr indPtr(new pcl::PointIndices);
        indPtr->indices = indIn.indices;
        condrem.setIndices(indPtr);
    }
    condrem.filter(*m_cloud[this->tmp]);

    std::vector<int> v;
    v.insert(v.end(), condrem.getRemovedIndices()->begin(), condrem.getRemovedIndices()->end());
    indOut->indices = v;
    // printf("outputIndices = %d pts . outputCloud = %d\n", indOut->indices.size(),m_cloud[this->tmp]->points.size());

    //printf("output = %d pts\n", m_cloud[inCloudId]->points.size()-indOut->indices.size());

    return 0;
}

int PclPointCloud::shadowPoints(double threshold, bool calcNormals, double radius, int k, int inCloudId /* = 0 */) {

    return 0;
}

int PclPointCloud::statOutlierRemoval(double mulThres, int k, int inCloudId /* = 0 */) {
    return 0;
}

int PclPointCloud::fastTriangulation(int maxNearest, double mu, double radius, int minAngle, int maxAngle, int maxAngleSurface, bool consistency, int inCloudId /* = 0 */) {

    printf("\ninput = %d pts\n", m_cloud[inCloudId]->points.size());
    printf("input = %d triangles\n", m_triangles.polygons.size());
    // Normal estimation    
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);

    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudxyz = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::copyPointCloud<pcl::PointXYZRGB, pcl::PointXYZ>(*(m_cloud[inCloudId]), *cloudxyz);
    tree->setInputCloud(cloudxyz);

    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
    n.setInputCloud(cloudxyz);
    n.setSearchMethod(tree);
    n.setKSearch(20);
    n.compute(*normals);
    //* normals should not contain the point normals + surface curvatures

    // Concatenate the XYZ and normal fields*
    pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
    pcl::concatenateFields(*cloudxyz, *normals, *cloud_with_normals);
    //* cloud_with_normals = cloud + normals

    // Create search tree*
    pcl::search::KdTree<pcl::PointNormal>::Ptr tree2(new pcl::search::KdTree<pcl::PointNormal>);
    tree2->setInputCloud(cloud_with_normals);

    // Initialize objects
    pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;

    gp3.setSearchRadius(radius);
    gp3.setMu(mu);
    gp3.setMaximumNearestNeighbors(maxNearest);
    gp3.setMaximumSurfaceAngle(maxAngleSurface * M_PI / 180);
    gp3.setMinimumAngle(minAngle * M_PI / 180);
    gp3.setMaximumAngle(maxAngle * M_PI / 180);
    gp3.setNormalConsistency(consistency);

    // Get result    
    m_triangles.polygons.clear();
    gp3.setInputCloud(cloud_with_normals);
    gp3.setSearchMethod(tree2);
    gp3.reconstruct(m_triangles);

    printf("output = %d triangles\n", m_triangles.polygons.size());

    *(m_cloud[triangulation]) = *(m_cloud[inCloudId]);
    showPointCloud(this->triangulation);

    return 0;
}

int PclPointCloud::movingLeastSquares(double fitRadius, int polyOrder, double upRadius, int numPtsInRadius, double voxelSize, int inCloudId /* = 0 */) {

    printf("\ninput = %d pts\n", m_cloud[inCloudId]->points.size());

    pcl::MovingLeastSquares<pcl::PointXYZRGB, pcl::PointXYZRGB> mls;

    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>);
    mls.setSearchMethod(tree);
    mls.setPolynomialFit(true);
    mls.setSearchRadius(fitRadius);
    mls.setPolynomialOrder(polyOrder);
    mls.setUpsamplingMethod(mls.NONE);

    if (upRadius != 0.0) {
        mls.setUpsamplingRadius(upRadius);
        mls.setUpsamplingStepSize(0.05);
        mls.setUpsamplingMethod(mls.SAMPLE_LOCAL_PLANE);
        printf("localplane\n");
    }
    else {
        if (numPtsInRadius != 0.0) {
            mls.setPointDensity(numPtsInRadius);
            mls.setUpsamplingMethod(mls.RANDOM_UNIFORM_DENSITY);
            printf("randomdensity\n");
        }
        else {
            if (voxelSize != 0.0) {
                mls.setDilationVoxelSize(voxelSize);
                mls.setDilationIterations(1);
                mls.setUpsamplingMethod(mls.VOXEL_GRID_DILATION);
                printf("voxeldilation\n");
            }
        }
    }
    m_cloud[this->mls]->clear();
    mls.setInputCloud(m_cloud[inCloudId]);
    mls.process(*m_cloud[this->mls]);

    for (int i = 0; i < m_cloud[this->mls]->points.size(); i++) {
        m_cloud[this->mls]->points.at(i).r = 255;
        m_cloud[this->mls]->points.at(i).g = 0;
        m_cloud[this->mls]->points.at(i).b = 0;
    }

    printf("output = %d pts\n", m_cloud[this->mls]->points.size());

    return 0;
}

int PclPointCloud::voxelGrid(double xSize, double ySize, double zSize, int inCloudId /* = 0 */) {

    printf("\ninput = %d pts\n", m_cloud[inCloudId]->points.size());

    pcl::VoxelGrid<pcl::PointXYZRGB> voxelGrid;

    voxelGrid.setLeafSize(xSize, ySize, zSize);
    voxelGrid.setDownsampleAllData(true);
    voxelGrid.setInputCloud(m_cloud[inCloudId]);
    voxelGrid.filter(*m_cloud[this->voxel_grid]);

    printf("output = %d pts\n", m_cloud[voxel_grid]->points.size());

    return 0;
}

int PclPointCloud::radiusOutlierRemoval(double radius, int minNeighbors, int inCloudId /* = 0 */) {

    printf("\ninput = %d pts\n", m_cloud[inCloudId]->points.size());

    pcl::RadiusOutlierRemoval<pcl::PointXYZRGB> ror;

    ror.setRadiusSearch(radius);
    ror.setMinNeighborsInRadius(minNeighbors);
    ror.setInputCloud(m_cloud[inCloudId]);
    ror.filter(*m_cloud[this->ror]);

    printf("output = %d pts\n", m_cloud[this->ror]->points.size());

    return 0;
}

int PclPointCloud::shapeContext3DEstimation(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudIn /* = NULL */, pcl::PointCloud<pcl::Normal>::Ptr cloudNormal /* = NULL */, pcl::PointCloud<pcl::ShapeContext1980>::Ptr cloudOut /* = NULL */, double minRadius /* = 0.5 */, double maxRadius /* = 0.5 */) {

    cloudOut->points.clear();
    pcl::ShapeContext3DEstimation<pcl::PointXYZRGB, pcl::Normal, pcl::ShapeContext1980> sc3de;
    sc3de.setInputCloud(cloudIn);
    sc3de.setInputNormals(cloudNormal);
    sc3de.setMinimalRadius(minRadius);
    sc3de.setRadiusSearch(maxRadius);
    sc3de.compute(*cloudOut);
    return 0;
}

int PclPointCloud::shapeContext3DEstimation(int cloudInId, pcl::PointCloud<pcl::ShapeContext1980>::Ptr cloudOut, double minRadius, double maxRadius) {

    for (int i = 0; i < m_cloudNormal[cloudInId]->points.size(); i++) {
        printf("%d normals = %f %f %f\n", i, m_cloudNormal[cloudInId]->points.at(i).normal_x, m_cloudNormal[cloudInId]->points.at(i).normal_y, m_cloudNormal[cloudInId]->points.at(i).normal_z);
    }


    this->shapeContext3DEstimation(m_cloud[cloudInId], m_cloudNormal[cloudInId], cloudOut, minRadius, maxRadius);
    return 0;
}

int PclPointCloud::principalCurvatures(string inPcdFile, string outTxtFile, string logFile, int kNN) {

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr inCloud = pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr outCloud = pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr(new pcl::PointCloud<pcl::PrincipalCurvatures>);
    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr normals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
    time_t time_end, time_st;

    if (pcl::io::loadPCDFile<pcl::PointXYZRGB> (inPcdFile, *(inCloud)) == -1) {
        printf("Error Reading File: %s\n", inPcdFile.data());
        return -1;
    }
    else {
        time_st = time(NULL);
        pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>);
        pcl::copyPointCloud<pcl::PointXYZRGB, pcl::PointXYZRGBNormal>(*(inCloud), *normals);

        tree->setInputCloud(inCloud);
        pcl::NormalEstimation<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> n;
        n.setInputCloud(inCloud);
        n.setSearchMethod(tree);
        n.setKSearch(kNN);
        n.compute(*normals);

        pcl::PrincipalCurvaturesEstimation<pcl::PointXYZRGB, pcl::PointXYZRGBNormal, pcl::PrincipalCurvatures> pce;
        pce.setInputCloud(inCloud);
        pce.setInputNormals(normals);
        pce.setKSearch(kNN);
        pce.compute(*outCloud);
        time_end = time(NULL);

        FILE *fdLog = fopen(logFile.data(), "a"); /*name in radNormal radCurv time ptsout*/
        fprintf(fdLog, "%s %d %d %d %d\n", inPcdFile.data(), inCloud->points.size(), kNN, time_end - time_st, outCloud->points.size());
        fclose(fdLog);

        //        FILE *fd = fopen(outTxtFile.data(), "w");        
        //        for(int i=0; i<outCloud->points.size();i++){
        //            pcl::PrincipalCurvatures pt = outCloud->points.at(i);
        //            fprintf(fd,"%f %f %f\n",pt.principal_curvature_x,pt.principal_curvature_y,pt.principal_curvature_z);
        //        }
        //        fclose(fd);

        printf("Curvatures:ptsIn = %d\nptsOut = %d\n", inCloud->points.size(), outCloud->points.size());

        char nn[200];
        string outName = inPcdFile.substr(0, inPcdFile.length() - 4);
        sprintf(nn, "%s_normals_k%02d.pcd", outName.data(), kNN);
        pcl::io::savePCDFileBinary(string(nn), *(normals));
        printf("saved: %s\n", nn);
        outName = inPcdFile.substr(0, inPcdFile.length() - 4);
        sprintf(nn, "%s_curvatures_k%02d.pcd", outName.data(), kNN);
        pcl::io::savePCDFileBinary(string(nn), *(outCloud));
        printf("saved: %s\n", nn);
        //        pcl::io::savePCDFileASCII(string("/media/DATOS/Videos/RealScenes/test_xyz420.pcd"), *(inCloud));
    }

    return 0;
}

int PclPointCloud::principalCurvatures(int inPcdId, string inPcdFile, string outTxtFile, string logFile, int kNN) {

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr inCloud = this->m_cloud[inPcdId];
    pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr outCloud = pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr(new pcl::PointCloud<pcl::PrincipalCurvatures>);
    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr normals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
    time_t time_end, time_st;


    time_st = time(NULL);
    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>);
    pcl::copyPointCloud<pcl::PointXYZRGB, pcl::PointXYZRGBNormal>(*(inCloud), *normals);

    tree->setInputCloud(inCloud);
    pcl::NormalEstimation<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> n;
    n.setInputCloud(inCloud);
    n.setSearchMethod(tree);
    n.setKSearch(kNN);
    n.compute(*normals);

    pcl::PrincipalCurvaturesEstimation<pcl::PointXYZRGB, pcl::PointXYZRGBNormal, pcl::PrincipalCurvatures> pce;
    pce.setInputCloud(inCloud);
    pce.setInputNormals(normals);
    pce.setKSearch(kNN);
    pce.compute(*outCloud);
    time_end = time(NULL);

    FILE *fdLog = fopen(logFile.data(), "a"); /*name in radNormal radCurv time ptsout*/
    fprintf(fdLog, "%s %d %d %d %d\n", inPcdFile.data(), inCloud->points.size(), kNN, time_end - time_st, outCloud->points.size());
    fclose(fdLog);

    //        FILE *fd = fopen(outTxtFile.data(), "w");        
    //        for(int i=0; i<outCloud->points.size();i++){
    //            pcl::PrincipalCurvatures pt = outCloud->points.at(i);
    //            fprintf(fd,"%f %f %f\n",pt.principal_curvature_x,pt.principal_curvature_y,pt.principal_curvature_z);
    //        }
    //        fclose(fd);

    printf("Curvatures:ptsIn = %d\nptsOut = %d\n", inCloud->points.size(), outCloud->points.size());

    char nn[200];
    string outName = inPcdFile.substr(0, inPcdFile.length() - 4);
    sprintf(nn, "%s_normals_k%02d.pcd", outName.data(), kNN);
    pcl::io::savePCDFileBinary(string(nn), *(normals));
    printf("saved: %s\n", nn);
    outName = inPcdFile.substr(0, inPcdFile.length() - 4);
    sprintf(nn, "%s_curvatures_k%02d.pcd", outName.data(), kNN);
    pcl::io::savePCDFileBinary(string(nn), *(outCloud));
    printf("saved: %s\n", nn);
    //        pcl::io::savePCDFileASCII(string("/media/DATOS/Videos/RealScenes/test_xyz420.pcd"), *(inCloud));


    return 0;
}

int PclPointCloud::principalCurvatures(string inPcdFile, string outTxtFile, string logFile, double radius /* = 0.25 */, double radNormal /* = 0.25 */) {

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr inCloud = pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr outCloud = pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr(new pcl::PointCloud<pcl::PrincipalCurvatures>);
    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr normals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
    time_t time_end, time_st;

    if (pcl::io::loadPCDFile<pcl::PointXYZRGB> (inPcdFile, *(inCloud)) == -1) {
        printf("Error Reading File: %s\n", inPcdFile.data());
        return -1;
    }
    else {
        time_st = time(NULL);
        pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>);
        pcl::copyPointCloud<pcl::PointXYZRGB, pcl::PointXYZRGBNormal>(*(inCloud), *normals);

        tree->setInputCloud(inCloud);
        pcl::NormalEstimation<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> n;
        n.setInputCloud(inCloud);
        n.setSearchMethod(tree);
        n.setRadiusSearch(radNormal);
        n.compute(*normals);

        pcl::PrincipalCurvaturesEstimation<pcl::PointXYZRGB, pcl::PointXYZRGBNormal, pcl::PrincipalCurvatures> pce;
        pce.setInputCloud(inCloud);
        pce.setInputNormals(normals);
        pce.setRadiusSearch(radius);
        pce.compute(*outCloud);
        time_end = time(NULL);

        FILE *fdLog = fopen(logFile.data(), "a"); /*name in radNormal radCurv time ptsout*/
        fprintf(fdLog, "%s %d %f %f %d %d\n", inPcdFile.data(), inCloud->points.size(), radNormal, radius, time_end - time_st, outCloud->points.size());
        fclose(fdLog);

        //        FILE *fd = fopen(outTxtFile.data(), "w");        
        //        for(int i=0; i<outCloud->points.size();i++){
        //            pcl::PrincipalCurvatures pt = outCloud->points.at(i);
        //            fprintf(fd,"%f %f %f\n",pt.principal_curvature_x,pt.principal_curvature_y,pt.principal_curvature_z);
        //        }
        //        fclose(fd);

        printf("Curvatures:ptsIn = %d\nptsOut = %d\n", inCloud->points.size(), outCloud->points.size());

        char nn[200];
        string outName = inPcdFile.substr(0, inPcdFile.length() - 4);
        sprintf(nn, "%s_normals_r%02d.pcd", outName.data(), (int) round(radius * 100));
        pcl::io::savePCDFileBinary(string(nn), *(normals));
        printf("saved: %s\n", nn);
        outName = inPcdFile.substr(0, inPcdFile.length() - 4);
        sprintf(nn, "%s_curvatures_r%02d.pcd", outName.data(), (int) round(radius * 100));
        pcl::io::savePCDFileBinary(string(nn), *(outCloud));
        printf("saved: %s\n", nn);
        //        pcl::io::savePCDFileASCII(string("/media/DATOS/Videos/RealScenes/test_xyz420.pcd"), *(inCloud));
    }

    return 0;
}

void PclPointCloud::showCurvatures(int cloudId, int opc, double limit) {
    /*
     * 0 camaras
     * 1 normal x
     * 2 normal y
     * 3 normal z
     * 4 normal curvature
     * 5 principal curvature x
     * 6 principal curvature y
     * 7 principal curvature z
     * 8 principal curvature max
     * 9 principal curvature min
     * 10 principal curvature max-min
     */

    int ind = 0;


    for (int j = 0; j < m_curvaturesCloud->points.size(); j++) {

        switch (opc) {
            case 1:
                ind = (int) round(fabs(m_xyzrgbnormalCloud->points.at(j).normal_x)*1000 / limit);
                //                    printf("[%05d]x(%.2f) y(%.2f) z(%.2f) data_c0(%f) data_c1(%f) data_c2(%f)\n",
                //                            j,
                //                            m_xyzrgbnormalCloud->points.at(j).x,
                //                            m_xyzrgbnormalCloud->points.at(j).y,
                //                            m_xyzrgbnormalCloud->points.at(j).z,
                //                            m_xyzrgbnormalCloud->points.at(j).normal_x,
                //                            m_xyzrgbnormalCloud->points.at(j).normal_y,
                //                            m_xyzrgbnormalCloud->points.at(j).normal_z);
                break;
            case 2:
                ind = (int) round(fabs(m_xyzrgbnormalCloud->points.at(j).normal_y)*1000 / limit);
                break;
            case 3:
                ind = (int) round(fabs(m_xyzrgbnormalCloud->points.at(j).normal_z)*1000 / limit);
                break;
            case 4:
                ind = (int) round(fabs(m_xyzrgbnormalCloud->points.at(j).curvature)*1000 / limit);
                break;
            case 5:
                ind = (int) round(fabs(m_curvaturesCloud->points.at(j).principal_curvature_x)*1000 / limit);
                break;
            case 6:
                ind = (int) round(fabs(m_curvaturesCloud->points.at(j).principal_curvature_y)*1000 / limit);
                break;
            case 7:
                ind = (int) round(fabs(m_curvaturesCloud->points.at(j).principal_curvature_z)*1000 / limit);
                break;
            case 8:
                ind = (int) round(fabs(m_curvaturesCloud->points.at(j).pc1)*1000 / limit);
                break;
            case 9:
                ind = (int) round(fabs(m_curvaturesCloud->points.at(j).pc2)*1000 / limit);
                break;
            case 10:
                ind = (int) round(fabs((m_curvaturesCloud->points.at(j).pc1 - m_curvaturesCloud->points.at(j).pc2))*1000 / limit);
                break;
        }

        if (ind >= m_red.size())
            ind = m_red.size() - 1;

        m_cloud[cloudId]->points.at(j).r = m_red.at(ind);
        m_cloud[cloudId]->points.at(j).g = m_green.at(ind);
        m_cloud[cloudId]->points.at(j).b = m_blue.at(ind);
    }

    this->showPointCloud(cloudId);
}

int PclPointCloud::readColorScale(string colorFile) {
    return readColorScale(colorFile, &m_red, &m_green, &m_blue);
}

int PclPointCloud::readColorScale(string colorFile, vector<int>* r, vector<int>* g, vector<int>* b) {

    FILE *fd = fopen(colorFile.data(), "r");
    if (fd != NULL) {
        r->clear();
        g->clear();
        b->clear();
        int rr, gg, bb;
        while (fscanf(fd, "%d\t%d\t%d\n", &rr, &gg, &bb) == 3) {
            r->push_back(rr);
            g->push_back(gg);
            b->push_back(bb);
        }
        fclose(fd);
        printf("%d elements read in color scale file\n", r->size());
    }
    else {

        printf("error reading color scale file: %s\n", colorFile.data());
    }
    return 0;
}

int PclPointCloud::principalCurvatures(int inPcdId, double radNormal /* = 0.25 */) {

    CpuTime cpu1, cpu2;
    cpu2.start();
    cpu1.start();
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr inCloud = this->m_cloud[inPcdId];
    pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr outCloud = pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr(new pcl::PointCloud<pcl::PrincipalCurvatures>);
    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr normals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
    pcl::copyPointCloud<pcl::PointXYZRGB, pcl::PointXYZRGBNormal>(*(inCloud), *normals);

    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>);

    tree->setInputCloud(inCloud);
    pcl::NormalEstimation<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> n;
    n.setInputCloud(inCloud);
    n.setSearchMethod(tree);
    n.setRadiusSearch(radNormal);
    n.compute(*normals);
    cpu1.stop();
    printf("Normals (R %.3f) = %s\n", radNormal, cpu1.getText().data());

    cpu1.start();
    pcl::PrincipalCurvaturesEstimation<pcl::PointXYZRGB, pcl::PointXYZRGBNormal, pcl::PrincipalCurvatures> pce;
    pce.setInputCloud(inCloud);
    pce.setInputNormals(normals);
    pce.setRadiusSearch(radNormal);
    pce.compute(*outCloud);
    cpu1.stop();
    printf("Curvatures (R %.3f) = %s\n", radNormal, cpu1.getText().data());

    m_curvaturesCloud = outCloud;
    this->m_xyzrgbnormalCloud = normals;

    cpu2.stop();
    printf("TOTAL = %s\n", cpu2.getText().data());

    printf("Curvatures:ptsIn = %d\nptsOut = %d\n", inCloud->points.size(), outCloud->points.size());

    return 0;
}

int PclPointCloud::principalCurvatures(int inPcdId, string inPcdFile, string outTxtFile, string logFile, double radius /* = 0.25 */, double radNormal /* = 0.25 */) {

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr inCloud = this->m_cloud[inPcdId];
    pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr outCloud = pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr(new pcl::PointCloud<pcl::PrincipalCurvatures>);
    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr normals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
    time_t time_end, time_st;


    time_st = time(NULL);
    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>);
    pcl::copyPointCloud<pcl::PointXYZRGB, pcl::PointXYZRGBNormal>(*(inCloud), *normals);

    tree->setInputCloud(inCloud);
    pcl::NormalEstimation<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> n;
    n.setInputCloud(inCloud);
    n.setSearchMethod(tree);
    n.setRadiusSearch(radNormal);
    n.compute(*normals);



    pcl::PrincipalCurvaturesEstimation<pcl::PointXYZRGB, pcl::PointXYZRGBNormal, pcl::PrincipalCurvatures> pce;
    pce.setInputCloud(inCloud);
    pce.setInputNormals(normals);
    pce.setRadiusSearch(radius);
    pce.compute(*outCloud);
    time_end = time(NULL);

    FILE *fdLog = fopen(logFile.data(), "a"); /*name in radNormal radCurv time ptsout*/
    fprintf(fdLog, "%s %d %f %f %d %d\n", inPcdFile.data(), inCloud->points.size(), radNormal, radius, time_end - time_st, outCloud->points.size());
    fclose(fdLog);

    //        FILE *fd = fopen(outTxtFile.data(), "w");        
    //        for(int i=0; i<outCloud->points.size();i++){
    //            pcl::PrincipalCurvatures pt = outCloud->points.at(i);
    //            fprintf(fd,"%f %f %f\n",pt.principal_curvature_x,pt.principal_curvature_y,pt.principal_curvature_z);
    //        }
    //        fclose(fd);

    printf("Curvatures:ptsIn = %d\nptsOut = %d\n", inCloud->points.size(), outCloud->points.size());

    char nn[200];
    string outName = inPcdFile.substr(0, inPcdFile.length() - 4);
    sprintf(nn, "%s_normals_r%02d.pcd", outName.data(), (int) round(radius * 100));
    pcl::io::savePCDFileBinary(string(nn), *(normals));
    printf("saved: %s\n", nn);
    outName = inPcdFile.substr(0, inPcdFile.length() - 4);
    sprintf(nn, "%s_curvatures_r%02d.pcd", outName.data(), (int) round(radius * 100));
    pcl::io::savePCDFileBinary(string(nn), *(outCloud));
    printf("saved: %s\n", nn);
    //        pcl::io::savePCDFileASCII(string("/media/DATOS/Videos/RealScenes/test_xyz420.pcd"), *(inCloud));


    return 0;
}

int PclPointCloud::floodFill_curvCloud(pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr dataCloud, int outCloudId, int startNode, double targetCurv, double lowThres, double highThres, int replaceR, int replaceG, int replaceB, double radius) {
    int ret = 0;
    vector<int> stack;
    vector<int> neighbors;
    vector<float> distances;
    stack.push_back(startNode);
    pcl::PrincipalCurvatures currData;

    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudxyz = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::copyPointCloud<pcl::PointXYZRGB, pcl::PointXYZ>(*(m_cloud[outCloudId]), *cloudxyz);
    tree->setInputCloud(cloudxyz);
    vector<bool> checked(cloudxyz->points.size(), false);
    vector<bool> inqueue(cloudxyz->points.size(), false);
    inqueue.at(stack.back()) = true;

    //    printf("stack[%d] = ",stack.size());
    //    for(int s=0; s<stack.size(); s++){
    //        printf("%d ",stack.at(s));
    //    }
    //    printf("\n");
    int iter = 0;

    while (stack.size() > 0) {
        iter++;
        //        printf("%d stack[%d] = ",iter++,stack.size());
        //        for(int s=0; s<stack.size(); s++){
        //            printf("%d ",stack.at(s));
        //        }
        //        printf("\n");

        neighbors.clear();
        distances.clear();
        int i = stack.back();
        stack.pop_back();
        currData = dataCloud->points.at(i);

        if (fabs(currData.principal_curvature_z) >= targetCurv - lowThres &&
            fabs(currData.principal_curvature_z) <= targetCurv + highThres &&
            !checked.at(i)) {
            //replace current node with the new color
            m_cloud[outCloudId]->points.at(i).r = replaceR;
            m_cloud[outCloudId]->points.at(i).g = replaceG;
            m_cloud[outCloudId]->points.at(i).b = replaceB;
            checked.at(i) = true;
            //search for neighbors            
            int maxnn = 0;
            int count = tree->radiusSearch(cloudxyz->points.at(i), radius, neighbors, distances, maxnn);
            //push them into stack
            if (count > 0 && count < cloudxyz->points.size()) {
                //                printf("%d neighbors of %d: ",count,i);
                for (int j = 0; j < count; j++) {
                    if (!inqueue.at(neighbors.at(j))) {
                        stack.push_back(neighbors.at(j));
                        inqueue.at(neighbors.at(j)) = true;
                        //printf("%d ",neighbors.at(j));
                    }
                }
                //                printf("\n");
            }
        }

    }
    printf("floodFillCurv finished after %d iterations\n", iter);
    return ret;
}

int PclPointCloud::floodFill_rgbCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr dataCloud, int outCloudId, int startNode, int targetR, int targetG, int targetB, int lowThres, int highThres, int replaceR, int replaceG, int replaceB, double radius) {

    int ret = 0;
    vector<int> stack;
    vector<int> neighbors;
    vector<float> distances;
    stack.push_back(startNode);
    pcl::PointXYZRGB currData;

    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudxyz = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::copyPointCloud<pcl::PointXYZRGB, pcl::PointXYZ>(*(m_cloud[outCloudId]), *cloudxyz);
    tree->setInputCloud(cloudxyz);
    vector<bool> checked(cloudxyz->points.size(), false);
    vector<bool> inqueue(cloudxyz->points.size(), false);
    inqueue.at(stack.back()) = true;
    dataCloud->points.at(startNode).r = targetR;
    dataCloud->points.at(startNode).g = targetG;
    dataCloud->points.at(startNode).b = targetB;

    //    printf("stack[%d] = ",stack.size());
    //    for(int s=0; s<stack.size(); s++){
    //        printf("%d ",stack.at(s));
    //    }
    //    printf("\n");
    int iter = 0;

    while (stack.size() > 0) {
        iter++;
        //        printf("%d stack[%d] = ",iter++,stack.size());
        //        for(int s=0; s<stack.size(); s++){
        //            printf("%d ",stack.at(s));
        //        }
        //        printf("\n");

        neighbors.clear();
        distances.clear();
        int i = stack.back();
        stack.pop_back();
        currData = dataCloud->points.at(i);

        if (currData.r >= targetR - lowThres &&
            currData.r <= targetR + highThres &&
            currData.g >= targetG - lowThres &&
            currData.g <= targetG + highThres &&
            currData.b >= targetB - lowThres &&
            currData.b <= targetB + highThres &&
            !checked.at(i)) {
            //replace current node with the new color
            m_cloud[outCloudId]->points.at(i).r = replaceR;
            m_cloud[outCloudId]->points.at(i).g = replaceG;
            m_cloud[outCloudId]->points.at(i).b = replaceB;
            checked.at(i) = true;
            //search for neighbors            
            int maxnn = 0;
            int count = tree->radiusSearch(cloudxyz->points.at(i), radius, neighbors, distances, maxnn);
            //push them into stack
            if (count > 0 && count < cloudxyz->points.size()) {
                //                printf("%d neighbors of %d: ",count,i);
                for (int j = 0; j < count; j++) {
                    if (!inqueue.at(neighbors.at(j))) {
                        stack.push_back(neighbors.at(j));
                        inqueue.at(neighbors.at(j)) = true;
                        //printf("%d ",neighbors.at(j));
                    }
                }
                //                printf("\n");
            }
        }

    }
    printf("floodFillrgb finished after %d iterations\n", iter);
    return ret;
}