CFaceDetection.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */
#include "detectors-precomp.h"  // Precompiled headers

#include <mrpt/detectors/CFaceDetection.h>
#include <mrpt/gui/CDisplayWindow.h>
#include <mrpt/gui/CDisplayWindow3D.h>
#include <mrpt/maps/CColouredPointsMap.h>
#include <mrpt/math/CMatrixDynamic.h>
#include <mrpt/math/CMatrixF.h>
#include <mrpt/math/geometry.h>
#include <mrpt/math/ops_containers.h>
#include <mrpt/opengl/CArrow.h>
#include <mrpt/opengl/CAxis.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/CPointCloudColoured.h>
#include <mrpt/opengl/CSetOfLines.h>
#include <mrpt/opengl/CSphere.h>
#include <mrpt/slam/CICP.h>
#include <mrpt/slam/CMetricMapsAlignmentAlgorithm.h>
#include <fstream>

using namespace std;
using namespace mrpt;
using namespace mrpt::detectors;
using namespace mrpt::math;
using namespace mrpt::img;
using namespace mrpt::gui;
using namespace mrpt::math;
using namespace mrpt::opengl;
using namespace mrpt::system;
using namespace mrpt::maps;
using namespace mrpt::obs;

//------------------------------------------------------------------------
//                          CFaceDetection
//------------------------------------------------------------------------
CFaceDetection::CFaceDetection()
{
    m_measure.numPossibleFacesDetected = 0;
    m_measure.numRealFacesDetected = 0;

    m_measure.faceNum = 0;

    m_timeLog.enable();
}

//------------------------------------------------------------------------
//                          ~CFaceDetection
//------------------------------------------------------------------------
CFaceDetection::~CFaceDetection()
{
    // Stop filters threads

    m_end_threads = true;

    m_enter_checkIfFacePlaneCov.set_value();
    m_enter_checkIfFaceRegions.set_value();
    m_enter_checkIfDiagonalSurface.set_value();

    m_thread_checkIfFaceRegions.join();
    m_thread_checkIfFacePlaneCov.join();
    m_thread_checkIfDiagonalSurface.join();
}

//------------------------------------------------------------------------
//                              init
//------------------------------------------------------------------------
void CFaceDetection::init(const mrpt::config::CConfigFileBase& cfg)
{
    m_options.confidenceThreshold =
        cfg.read_int("FaceDetection", "confidenceThreshold", 240);
    m_options.multithread = cfg.read_bool("FaceDetection", "multithread", true);
    m_options.useCovFilter =
        cfg.read_bool("FaceDetection", "useCovFilter", true);
    m_options.useRegionsFilter =
        cfg.read_bool("FaceDetection", "useRegionsFilter", true);
    m_options.useSizeDistanceRelationFilter =
        cfg.read_bool("FaceDetection", "useSizeDistanceRelationFilter", true);
    m_options.useDiagonalDistanceFilter =
        cfg.read_bool("FaceDetection", "useDiagonalDistanceFilter", true);

    m_testsOptions.planeThreshold =
        cfg.read_double("FaceDetection", "planeThreshold", 50);
    m_testsOptions.planeTest_eigenVal_top =
        cfg.read_double("FaceDetection", "planeTest_eigenVal_top", 0.011);
    m_testsOptions.planeTest_eigenVal_bottom =
        cfg.read_double("FaceDetection", "planeTest_eigenVal_bottom", 0.0002);
    m_testsOptions.regionsTest_sumDistThreshold_top = cfg.read_double(
        "FaceDetection", "regionsTest_sumDistThreshold_top", 0.5);
    m_testsOptions.regionsTest_sumDistThreshold_bottom = cfg.read_double(
        "FaceDetection", "regionsTest_sumDistThreshold_bottom", 0.04);

    m_measure.takeTime = cfg.read_bool("FaceDetection", "takeTime", false);
    m_measure.takeMeasures =
        cfg.read_bool("FaceDetection", "takeMeasures", false);
    m_measure.saveMeasurementsToFile =
        cfg.read_bool("FaceDetection", "saveMeasurementsToFile", false);

    // Run filters threads
    if (m_options.multithread)
    {
        if (m_options.useRegionsFilter)
            m_thread_checkIfFaceRegions =
                std::thread(dummy_checkIfFaceRegions, this);
        if (m_options.useCovFilter)
            m_thread_checkIfFacePlaneCov =
                std::thread(dummy_checkIfFacePlaneCov, this);
        if (m_options.useSizeDistanceRelationFilter ||
            m_options.useDiagonalDistanceFilter)
            m_thread_checkIfDiagonalSurface =
                std::thread(dummy_checkIfDiagonalSurface, this);

        m_checkIfFacePlaneCov_res = false;
        m_checkIfFaceRegions_res = true;
        m_checkIfDiagonalSurface_res = true;
    }

    cascadeClassifier.init(cfg);
}

//------------------------------------------------------------------------
//                          detectObjects
//------------------------------------------------------------------------
void CFaceDetection::detectObjects_Impl(
    const mrpt::obs::CObservation& obs, vector_detectable_object& detected)
{
    MRPT_START

    // Detect possible faces
    vector_detectable_object localDetected;

    // To obtain experimental results
    {
        if (m_measure.takeTime) m_timeLog.enter("Detection time");
    }

    cascadeClassifier.detectObjects(obs, localDetected);

    // To obtain experimental results
    {
        if (m_measure.takeTime) m_timeLog.leave("Detection time");

        // if ( m_measure.takeMeasures )
        m_measure.numPossibleFacesDetected += localDetected.size();
    }

    // Check if we are using a 3D Camera and 3D points are saved
    if ((IS_CLASS(obs, CObservation3DRangeScan)) && (localDetected.size() > 0))
    {
        // To obtain experimental results
        {
            if (m_measure.takeTime) m_timeLog.enter("Check if real face time");
        }

        auto& o = static_cast<CObservation3DRangeScan&>(
            const_cast<CObservation&>(obs));

        if (o.hasPoints3D)
        {
            // Vector to save detected objects to delete if they aren't a face
            vector<size_t> deleteDetected;

            // Check if all possible detected faces satisfy a serial of
            // constrains
            for (unsigned int i = 0; i < localDetected.size(); i++)
            {
                CDetectable2D::Ptr rec =
                    std::dynamic_pointer_cast<CDetectable2D>(localDetected[i]);

                // Calculate initial and final rows and columns
                unsigned int r1 = rec->m_y;
                unsigned int r2 = rec->m_y + rec->m_height;
                unsigned int c1 = rec->m_x;
                unsigned int c2 = rec->m_x + rec->m_width;

                o.getZoneAsObs(m_lastFaceDetected, r1, r2, c1, c2);

                if (m_options.multithread)
                {
                    // To obtain experimental results
                    {
                        if (m_measure.takeTime)
                            m_timeLog.enter("Multithread filters application");
                    }

                    // Semaphores signal
                    if (m_options.useCovFilter)
                        m_enter_checkIfFacePlaneCov.set_value();
                    if (m_options.useRegionsFilter)
                        m_enter_checkIfFaceRegions.set_value();
                    if (m_options.useSizeDistanceRelationFilter ||
                        m_options.useDiagonalDistanceFilter)
                        m_enter_checkIfDiagonalSurface.set_value();

                    // Semaphores wait
                    if (m_options.useCovFilter)
                        m_leave_checkIfFacePlaneCov.get_future().wait();
                    if (m_options.useRegionsFilter)
                        m_leave_checkIfFaceRegions.get_future().wait();
                    if (m_options.useSizeDistanceRelationFilter ||
                        m_options.useDiagonalDistanceFilter)
                        m_leave_checkIfDiagonalSurface.get_future().wait();

                    // Check resutls
                    if (!m_checkIfFacePlaneCov_res ||
                        !m_checkIfFaceRegions_res ||
                        !m_checkIfDiagonalSurface_res)
                        deleteDetected.push_back(i);

                    // To obtain experimental results
                    {
                        if (m_measure.takeTime)
                            m_timeLog.leave("Multithread filters application");
                    }

                    m_measure.faceNum++;
                }
                else
                {
                    // To obtain experimental results
                    {
                        if (m_measure.takeTime)
                            m_timeLog.enter("Secuential filters application");
                    }

                    /////////////////////////////////////////////////////
                    // CMatrixDynamic<bool> region;
                    // experimental_segmentFace( m_lastFaceDetected,  region);
                    /////////////////////////////////////////////////////

                    // m_lastFaceDetected.intensityImage.saveToFile(format("%i.jpg",m_measure.faceNum));

                    bool remove = false;

                    // First check if we can adjust a plane to detected region
                    // as face, if yes it isn't a face!
                    if (m_options.useCovFilter &&
                        !checkIfFacePlaneCov(&m_lastFaceDetected))
                    {
                        deleteDetected.push_back(i);
                        remove = true;
                    }
                    else if (
                        m_options.useRegionsFilter &&
                        !checkIfFaceRegions(&m_lastFaceDetected))
                    {
                        deleteDetected.push_back(i);
                        remove = true;
                    }
                    else if (
                        (m_options.useSizeDistanceRelationFilter ||
                         m_options.useDiagonalDistanceFilter) &&
                        !checkIfDiagonalSurface(&m_lastFaceDetected))
                    {
                        deleteDetected.push_back(i);
                        remove = true;
                    }

                    if (remove)
                    {
                        /*ofstream f;
                        f.open("deleted.txt", ofstream::app);
                        f << "Deleted: " << m_measure.faceNum << endl;
                        f.close();*/
                        m_measure.deletedRegions.push_back(m_measure.faceNum);
                    }

                    m_measure.faceNum++;

                    // To obtain experimental results
                    {
                        if (m_measure.takeTime)
                            m_timeLog.leave("Secuential filters application");
                    }
                }
            }

            // Delete non faces
            for (unsigned int i = deleteDetected.size(); i > 0; i--)
                localDetected.erase(
                    localDetected.begin() + deleteDetected[i - 1]);
        }

        // Convert 2d detected objects to 3d
        for (const auto& i : localDetected)
        {
            CDetectable3D::Ptr object3d = CDetectable3D::Ptr(
                new CDetectable3D(std::dynamic_pointer_cast<CDetectable2D>(i)));
            detected.push_back(object3d);
        }

        // To obtain experimental results
        {
            if (m_measure.takeTime) m_timeLog.leave("Check if real face time");
        }
    }
    else  // Not using a 3D camera
    {
        detected = localDetected;
    }

    // To obtain experimental results
    {
        // if ( m_measure.takeMeasures )
        m_measure.numRealFacesDetected += detected.size();
    }

    MRPT_END
}

//------------------------------------------------------------------------
//                          checkIfFacePlane
//------------------------------------------------------------------------
bool CFaceDetection::checkIfFacePlane(CObservation3DRangeScan* face)
{
    vector<TPoint3D> points;

    size_t N = face->points3D_x.size();

    points.resize(N);

    for (size_t i = 0; i < N; i++)
        points[i] = TPoint3D(
            face->points3D_x.at(i), face->points3D_y.at(i),
            face->points3D_z.at(i));

    // Try to ajust a plane
    TPlane plane;

    // To obtain experimental results
    {
        if (m_measure.takeMeasures)
            m_measure.errorEstimations.push_back(
                (double)getRegressionPlane(points, plane));
    }

    if (getRegressionPlane(points, plane) < m_testsOptions.planeThreshold)
        return true;

    return false;
}

void CFaceDetection::dummy_checkIfFacePlaneCov(CFaceDetection* obj)
{
    obj->thread_checkIfFacePlaneCov();
}

void CFaceDetection::thread_checkIfFacePlaneCov()
{
    for (;;)
    {
        m_enter_checkIfFacePlaneCov.get_future().wait();

        if (m_end_threads) break;

        // Perform filter
        m_checkIfFacePlaneCov_res = checkIfFacePlaneCov(&m_lastFaceDetected);

        m_leave_checkIfFacePlaneCov.set_value();
    }
}

//------------------------------------------------------------------------
//                       checkIfFacePlaneCov
//------------------------------------------------------------------------
bool CFaceDetection::checkIfFacePlaneCov(CObservation3DRangeScan* face)
{
    MRPT_TRY_START

    // To obtain experimental results
    {
        if (m_measure.takeTime)
            m_timeLog.enter("Check if face plane: covariance");
    }

    // Get face region size
    const unsigned int faceWidth = face->intensityImage.getWidth();
    const unsigned int faceHeight = face->intensityImage.getHeight();

    // We work with a confidence image?
    const bool confidence = face->hasConfidenceImage;

    // To fill with valid points
    vector<CVectorFixedDouble<3>> pointsVector;

    CMatrixDynamic<bool> region;  // To save the segmented region
    experimental_segmentFace(*face, region);

    for (unsigned int j = 0; j < faceHeight; j++)
    {
        for (unsigned int k = 0; k < faceWidth; k++)
        {
            CVectorFixedDouble<3> aux;

            // Don't take in account dark pixels
            if (region(j, k) &&
                (((!confidence) ||
                  ((confidence) &&
                   (face->confidenceImage.at<uint8_t>(k, j) >
                    m_options.confidenceThreshold) &&
                   (face->intensityImage.at<uint8_t>(k, j) > 50)))))
            {
                int position = faceWidth * j + k;
                aux[0] = face->points3D_x[position];
                aux[1] = face->points3D_y[position];
                aux[2] = face->points3D_z[position];
                pointsVector.push_back(aux);
            }
        }
    }

    // Check if points vector is empty to avoid a future crash
    if (pointsVector.empty()) return false;

    // experimental_viewFacePointsScanned( *face );

    // To obtain the covariance vector and eigenvalues
    CMatrixDouble cov;
    CMatrixDouble eVects;
    std::vector<double> eVals;

    cov = covVector<vector<CVectorFixedDouble<3>>, CMatrixDouble>(pointsVector);

    cov.eig(eVects, eVals);

    // To obtain experimental results
    {
        if (m_measure.takeMeasures) m_measure.lessEigenVals.push_back(eVals[0]);

        if (m_measure.takeTime)
            m_timeLog.leave("Check if face plane: covariance");

        // Uncomment if you want to analyze the calculated eigenvalues
        // ofstream f;
        /*f.open("eigenvalues.txt", ofstream::app);
        f << m_measure.faceNum << " " << eVals[0] << endl;
        f.close();*/

        // f.open("eigenvalues2.txt", ofstream::app);
        cout << eVals[0] << " " << eVals[1] << " " << eVals[2] << " > ";
        cout << eVals[0] / eVals[2] << endl;
        // f << eVals[0]/eVals[2] << endl;
        // f.close();
    }

    if (m_measure.faceNum >= 314)
        experimental_viewFacePointsAndEigenVects(pointsVector, eVects, eVals);

    // Check if the less eigenvalue is out of the permited area
    // if ( ( eVals[0] > m_options.planeEigenValThreshold_down )
    //  && ( eVals[0] < m_options.planeEigenValThreshold_up ) )
    if (eVals[0] / eVals[2] > 0.06)
    {
        // Uncomment if you want to save the face regions discarted by this
        // filter
        /*ofstream f;
        f.open("deletedCOV.txt", ofstream::app);
        f << m_measure.faceNum << endl;
        f.close();*/

        return true;  // Filter not passed
    }

    return false;  // Filter passed

    MRPT_TRY_END
}

void CFaceDetection::dummy_checkIfFaceRegions(CFaceDetection* obj)
{
    obj->thread_checkIfFaceRegions();
}

void CFaceDetection::thread_checkIfFaceRegions()
{
    for (;;)
    {
        m_enter_checkIfFaceRegions.get_future().wait();

        if (m_end_threads) break;

        // Perform filter
        m_checkIfFaceRegions_res = checkIfFaceRegions(&m_lastFaceDetected);

        m_leave_checkIfFaceRegions.set_value();
    }
}

//------------------------------------------------------------------------
//                          checkIfFaceRegions
//------------------------------------------------------------------------

bool CFaceDetection::checkIfFaceRegions(CObservation3DRangeScan* face)
{
    MRPT_START

    // To obtain experimental results
    {
        if (m_measure.takeTime) m_timeLog.enter("Check if face plane: regions");
    }

    // To obtain region size
    const unsigned int faceWidth = face->intensityImage.getWidth();
    const unsigned int faceHeight = face->intensityImage.getHeight();

    // Initial vertical size of a region
    unsigned int sectionVSize = faceHeight / 3.0;

    // Steps of this filter
    //  1. To segment the region detected as face using a regions growing
    // algorithm
    //  2. To obtain the first and last column to work (a profile face detected
    // can have a lateral area without to use)
    //  3. To calculate the histogram of the upper zone of the region for
    // determine if we use it (if this zone present
    //      a lot of dark pixels the measurements can be wrong)
    //  4. To obtain the coordinates of pixels that form each subregion
    //  5. To calculate medians or means of each subregion
    //  6. To check subregions constrains

    vector<TPoint3D> points;

    TPoint3D meanPos[3][3] = {
        {TPoint3D(0, 0, 0), TPoint3D(0, 0, 0), TPoint3D(0, 0, 0)},
        {TPoint3D(0, 0, 0), TPoint3D(0, 0, 0), TPoint3D(0, 0, 0)},
        {TPoint3D(0, 0, 0), TPoint3D(0, 0, 0), TPoint3D(0, 0, 0)}};
    int numPoints[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};

    vector<TPoint3D> regions2[9];

    //
    //  1. To segment the region detected as face using a regions growing
    // algorithm
    //

    CMatrixDynamic<bool> region;  // To save the segmented region
    experimental_segmentFace(*face, region);

    //
    //  2. To obtain the first and last column to work (a profile face detected
    // can have a lateral area without to use)
    //

    size_t start = faceWidth, end = 0;

    for (size_t r = 0; r < region.rows(); r++)
        for (size_t c = 1; c < region.cols(); c++)
        {
            if ((!(region(r, c - 1))) && (region(r, c)))
            {
                if (c < start) start = c;
            }
            else if ((region(r, c - 1)) && (!(region(r, c))))
                if (c > end) end = c;

            if ((c > end) && (region(r, c))) end = c;
        }

    if (end == 0) end = faceWidth - 1;  // Check if the end has't changed
    if (end < 3 * (faceWidth / 4))
        end = 3 * (faceWidth / 4);  // To avoid spoiler
    if (start == faceWidth) start = 0;  // Check if the start has't changed
    if (start > faceWidth / 4) start = faceWidth / 4;  // To avoid spoiler

    // cout << "Start: " << start << " End: " << end << endl;

    // To use the start and end calculated to obtain the final regions limits
    unsigned int utilWidth = faceWidth - start - (faceWidth - end);
    unsigned int c1 = ceil(utilWidth / 3.0 + start);
    unsigned int c2 = ceil(2 * (utilWidth / 3.0) + start);

    //
    //  3. To calculate the histogram of the upper zone of the region for
    // determine if we use it
    //

    CMatrixDynamic<unsigned int> hist;
    hist.setSize(1, 256, true);
    experimental_calcHist(
        face->intensityImage, start, 0, end, ceil(faceHeight * 0.1), hist);

    size_t countHist = 0;
    for (size_t i = 0; i < 60; i++)
    {
        countHist += hist(0, i);
    }

    size_t upLimit = 0;
    size_t downLimit = faceHeight - 1;

    if (countHist > 10)
    {
        upLimit = floor(faceHeight * 0.1);
        downLimit = floor(faceHeight * 0.9);
    }

    // Uncomment it if you want to analyze the number of pixels that have more
    // dark that the 60 gray tone
    // m_meanHist.push_back( countHist );

    //
    //  4. To obtain the coordinates of pixels that form each region
    //

    unsigned int cont = 0;

    for (unsigned int r = 0; r < faceHeight; r++)
    {
        for (unsigned int c = 0; c < faceWidth; c++, cont++)
        {
            if ((r >= upLimit) && (r <= downLimit) && (region(r, c)) &&
                (face->confidenceImage.at<uint8_t>(c, r, 0) >
                 m_options.confidenceThreshold) &&
                (face->intensityImage.at<uint8_t>(c, r)) > 50)
            {
                unsigned int row, col;
                if (r < sectionVSize + upLimit * 0.3)
                    row = 0;
                else if (r < sectionVSize * 2 - upLimit * 0.15)
                    row = 1;
                else
                    row = 2;

                if (c < c1)
                    col = 0;
                else if (c < c2)
                    col = 1;
                else
                    col = 2;

                TPoint3D point(
                    face->points3D_x[cont], face->points3D_y[cont],
                    face->points3D_z[cont]);
                meanPos[row][col] = meanPos[row][col] + point;

                ++numPoints[row][col];

                if (row == 0 && col == 0)
                    regions2[0].emplace_back(
                        face->points3D_x[cont], face->points3D_y[cont],
                        face->points3D_z[cont]);
                else if (row == 0 && col == 1)
                    regions2[1].emplace_back(
                        face->points3D_x[cont], face->points3D_y[cont],
                        face->points3D_z[cont]);
                else if (row == 0 && col == 2)
                    regions2[2].emplace_back(
                        face->points3D_x[cont], face->points3D_y[cont],
                        face->points3D_z[cont]);
                else if (row == 1 && col == 0)
                    regions2[3].emplace_back(
                        face->points3D_x[cont], face->points3D_y[cont],
                        face->points3D_z[cont]);
                else if (row == 1 && col == 1)
                    regions2[4].emplace_back(
                        face->points3D_x[cont], face->points3D_y[cont],
                        face->points3D_z[cont]);
                else if (row == 1 && col == 2)
                    regions2[5].emplace_back(
                        face->points3D_x[cont], face->points3D_y[cont],
                        face->points3D_z[cont]);
                else if (row == 2 && col == 0)
                    regions2[6].emplace_back(
                        face->points3D_x[cont], face->points3D_y[cont],
                        face->points3D_z[cont]);
                else if (row == 2 && col == 1)
                    regions2[7].emplace_back(
                        face->points3D_x[cont], face->points3D_y[cont],
                        face->points3D_z[cont]);
                else
                    regions2[8].emplace_back(
                        face->points3D_x[cont], face->points3D_y[cont],
                        face->points3D_z[cont]);
            }
        }
    }

    //
    //  5. To calculate medians or means of each subregion
    //

    vector<double> oldPointsX1;

    size_t middle1 = 0;
    size_t middle2 = 0;

    if (regions2[0].size() > 0)
    {
        for (auto& i : regions2[0]) oldPointsX1.push_back(i.x);

        middle1 = floor((double)oldPointsX1.size() / 2);
        nth_element(
            oldPointsX1.begin(), oldPointsX1.begin() + middle1,
            oldPointsX1.end());  // Obtain center element
    }

    vector<double> oldPointsX2;

    if (regions2[2].size() > 0)
    {
        for (auto& i : regions2[2]) oldPointsX2.push_back(i.x);

        middle2 = floor((double)oldPointsX2.size() / 2);
        nth_element(
            oldPointsX2.begin(), oldPointsX2.begin() + middle2,
            oldPointsX2.end());  // Obtain center element
    }

    for (size_t i = 0; i < 3; i++)
        for (size_t j = 0; j < 3; j++)
            if (!numPoints[i][j])
                meanPos[i][j] = TPoint3D(0, 0, 0);
            else
                meanPos[i][j] = meanPos[i][j] / numPoints[i][j];

    if (regions2[0].size() > 0) meanPos[0][0].x = oldPointsX1.at(middle1);

    if (regions2[2].size() > 0) meanPos[0][2].x = oldPointsX2.at(middle2);

    //
    //  6. To check subregions constrains
    //
    vector<double> dist(5);
    size_t res = checkRelativePosition(
        meanPos[1][0], meanPos[1][2], meanPos[1][1], dist[0]);
    res += res && checkRelativePosition(
                      meanPos[2][0], meanPos[2][2], meanPos[2][1], dist[1]);
    res += res && checkRelativePosition(
                      meanPos[0][0], meanPos[0][2], meanPos[0][1], dist[2]);
    res += res && checkRelativePosition(
                      meanPos[0][0], meanPos[2][2], meanPos[1][1], dist[3]);
    res += res && checkRelativePosition(
                      meanPos[2][0], meanPos[0][2], meanPos[1][1], dist[4]);

    ofstream f;
    f.open("dist.txt", ofstream::app);
    f << sum(dist) << endl;
    f.close();

    bool real = false;
    if (!res)
        real = true;
    else if ((res = 1) && (sum(dist) > 0.04))
        real = true;

    f.open("tam.txt", ofstream::app);
    f << meanPos[0][1].distanceTo(meanPos[2][1]) << endl;
    f.close();

    // experimental_viewRegions( regions2, meanPos );

    // cout << endl << meanPos[0][0] << "\t" << meanPos[0][1] << "\t" <<
    // meanPos[0][2];
    //  cout << endl << meanPos[1][0] << "\t" << meanPos[1][1] << "\t" <<
    // meanPos[1][2];
    //  cout << endl << meanPos[2][0] << "\t" << meanPos[2][1] << "\t" <<
    // meanPos[2][2] << endl;

    // To obtain experimental results
    {
        if (m_measure.takeTime) m_timeLog.leave("Check if face plane: regions");
    }

    if (real)
        return true;  // Filter passed
    else
    {
        // Uncomment if you want to known what regions was discarted by this
        // filter
        /*ofstream f;
        f.open("deletedSTRUCTURES.txt", ofstream::app);
        f << m_measure.faceNum << endl;
        f.close();*/

        return false;  // Filter not passed
    }

    MRPT_END
}

//------------------------------------------------------------------------
//                          checkRelativePosition
//------------------------------------------------------------------------

size_t CFaceDetection::checkRelativePosition(
    const TPoint3D& p1, const TPoint3D& p2, const TPoint3D& p, double& dist)
{
    double x1 = -p1.y;
    double y1 = p1.x;

    double x2 = -p2.y;
    double y2 = p2.x;

    double x = -p.y;
    double y = p.x;

    double yIdeal = y1 + (((x - x1) * (y2 - y1)) / (x2 - x1));

    //////////////////////////////////

    /*double xaux = x2;
    double yaux = y1;

    cout << "Grados= " << RAD2DEG(acos(
    (xaux-x1)/(sqrt(pow(x1-x2,2)+pow(y1-y2,2))) )) << endl;*/

    ///////////////////////////////////////

    dist = yIdeal - y;

    if (y < yIdeal)
        return 0;
    else
        return 1;
}

void CFaceDetection::dummy_checkIfDiagonalSurface(CFaceDetection* obj)
{
    obj->thread_checkIfDiagonalSurface();
}

void CFaceDetection::thread_checkIfDiagonalSurface()
{
    for (;;)
    {
        m_enter_checkIfDiagonalSurface.get_future().wait();

        if (m_end_threads) break;

        // Perform filter
        m_checkIfDiagonalSurface_res =
            checkIfDiagonalSurface(&m_lastFaceDetected);

        m_leave_checkIfDiagonalSurface.set_value();
    }
}

//------------------------------------------------------------------------
//                          checkIfDiagonalSurface
//------------------------------------------------------------------------

bool CFaceDetection::checkIfDiagonalSurface(CObservation3DRangeScan* face)
{
    MRPT_START

    // To obtain experimental results
    {
        if (m_options.useDiagonalDistanceFilter && m_measure.takeTime)
            m_timeLog.enter("Check if face plane: diagonal distances");

        if (m_options.useSizeDistanceRelationFilter && m_measure.takeTime)
            m_timeLog.enter("Check if face plane: size-distance relation");
    }

    const unsigned int faceWidth = face->intensityImage.getWidth();
    const unsigned int faceHeight = face->intensityImage.getHeight();

    // const float max_desv = 0.2;

    unsigned int x1 = ceil(faceWidth * 0.25);
    unsigned int x2 = floor(faceWidth * 0.75);
    unsigned int y1 = ceil(faceHeight * 0.15);
    unsigned int y2 = floor(faceHeight * 0.85);

    vector<TPoint3D> points;
    unsigned int cont = (y1 == 0 ? 0 : faceHeight * (y1 - 1));
    CMatrixBool valids;

    valids.setSize(faceHeight, faceWidth);

    int total = 0;
    double sumDepth = 0;

    for (unsigned int i = y1; i <= y2; i++)
    {
        cont += x1;

        for (unsigned int j = x1; j <= x2; j++, cont++)
        {
            if (face->confidenceImage.at<uint8_t>(j, i, 0) >
                m_options.confidenceThreshold)
            {
                sumDepth += face->points3D_x[cont];
                total++;
                points.emplace_back(
                    face->points3D_x[cont], face->points3D_y[cont],
                    face->points3D_z[cont]);
            }
        }
        cont += faceWidth - x2 - 1;
    }

    double meanDepth = sumDepth / total;

    /*if (  m_measure.faceNum == 434  )
        experimental_viewFacePointsScanned( *face );*/

    // experimental_viewFacePointsScanned( points );

    bool res = true;

    if (m_options.useSizeDistanceRelationFilter)
    {
        double maxFaceDistance = 0.5 + 1000 / (pow(faceWidth, 1.9));

        // To obtain experimental results
        {
            if (m_measure.takeTime)
                m_timeLog.leave("Check if face plane: size-distance relation");

            if (m_options.useDiagonalDistanceFilter && m_measure.takeTime)
                m_timeLog.leave("Check if face plane: diagonal distances");
        }

        /*if ( maxFaceDistance > meanDepth )
            return true;

        if ( !m_options.useDiagonalDistanceFilter )
            return false;*/

        if (maxFaceDistance < meanDepth)
        {
            // Uncomment if you want to analyze the regions discarted by this
            // filter
            /*ofstream f;
            f.open("deletedSIZEDISTANCE.txt", ofstream::app);
            f << m_measure.faceNum << endl;
            f.close();*/

            // if ( !m_options.useDiagonalDistanceFilter )
            return false;
            // else
            //  res = false;
        }

        if (!m_options.useDiagonalDistanceFilter) return true;
    }

    ofstream f;
    /*f.open("relaciones1.txt", ofstream::app);
    f << faceWidth << endl;
    f.close();*/

    f.open("relaciones2.txt", ofstream::app);
    f << meanDepth << endl;
    f.close();

    // cout << m_measure.faceNum ;

    // experimental_viewFacePointsScanned( points );

    points.clear();

    cont = (y1 == 1 ? 0 : faceHeight * (y1 - 1));

    for (unsigned int i = y1; i <= y2; i++)
    {
        cont += x1;

        for (unsigned int j = x1; j <= x2; j++, cont++)
        {
            if ((face->confidenceImage.at<uint8_t>(j, i, 0) >
                 m_options.confidenceThreshold))
            //&& ( face->points3D_x[cont] > meanDepth - max_desv )
            //&& ( face->points3D_x[cont] < meanDepth + max_desv ) )
            {
                valids(i, j) = true;
                points.emplace_back(
                    face->points3D_x[cont], face->points3D_y[cont],
                    face->points3D_z[cont]);
            }
            else
                valids(i, j) = false;
        }
        cont += faceWidth - x2 - 1;
    }

    /*if (  m_measure.faceNum > 838 )
        experimental_viewFacePointsScanned( points );*/

    // if ( ( m_measure.faceNum == 225 ) || ( m_measure.faceNum == 226 ) )
    // experimental_viewFacePointsScanned( points );

    double sumDistances = 0;
    double distance;
    int offsetIndex;

    cont = 0;

    for (unsigned int i = y1; i <= y2; i++)
    {
        cont += x1;

        for (unsigned int j = x1; j <= x2; j++, cont++)
        {
            if (valids(i, j))
            {
                // experimental_calcDiagDist( face, i, j, faceWidth, faceHeight,
                // valids, distance );

                distance = 0;
                if ((i + 1 <= y2) && (j + 1 <= x2))
                {
                    if (valids(i + 1, j + 1))
                    {
                        TPoint3D p1(
                            face->points3D_x[cont], face->points3D_y[cont],
                            face->points3D_z[cont]);
                        offsetIndex = cont + faceWidth + 1;
                        distance = p1.distanceTo(TPoint3D(
                            face->points3D_x[offsetIndex],
                            face->points3D_y[offsetIndex],
                            face->points3D_z[offsetIndex]));
                    }
                    else
                    {
                        bool validOffset = true;
                        int offset = 2;

                        while (validOffset)
                        {
                            if ((i + offset <= y2) && (j + offset <= x2))
                            {
                                if (valids(i + offset, j + offset))
                                {
                                    TPoint3D p1(
                                        face->points3D_x[cont],
                                        face->points3D_y[cont],
                                        face->points3D_z[cont]);
                                    offsetIndex = cont + faceWidth + offset;
                                    distance = p1.distanceTo(TPoint3D(
                                        face->points3D_x[offsetIndex],
                                        face->points3D_y[offsetIndex],
                                        face->points3D_z[offsetIndex]));
                                    break;
                                }
                                offset++;
                            }
                            else
                                validOffset = false;
                        }
                    }
                }

                sumDistances += distance;
            }
        }
        cont += faceWidth - x2 - 1;
    }

    // For experimental results
    {
        if (m_measure.takeMeasures)
            m_measure.sumDistances.push_back(sumDistances);

        ofstream fo;
        fo.open("distances.txt", ofstream::app);
        // f << m_measure.faceNum << " " << sumDistances << endl;
        fo << sumDistances << endl;
        fo.close();

        fo.open("distances2.txt", ofstream::app);
        fo << m_measure.faceNum << " " << sumDistances << endl;
        fo.close();
    }

    // double yMax = 3 + 3.8 / ( pow( meanDepth, 2 ) );
    // double yMax = 3 + 7 /( pow( meanDepth, 2) ) ;
    double yMax = 3 + 6 / (pow(meanDepth, 2));
    double yMin = 1 + 3.8 / (pow(meanDepth + 1.2, 2));

    // To obtain experimental results
    {
        if (m_measure.takeTime)
            m_timeLog.leave("Check if face plane: diagonal distances");
    }

    if (((sumDistances <= yMax) && (sumDistances >= yMin)) && (res))
    {
        /* Uncomment if you want to analyze the real size of each studied region
        / *ofstream f;
        f.open("sizes.txt", ofstream::app);
        double h = meanDepth/cos(DEG2RAD(faceHeight*0.2361111111111111));
        double realHigh = sin(DEG2RAD(faceHeight*0.2361111111111111))*h;
        f << realHigh << endl;
        f.close();*/

        return true;
    }

    // Uncomment if you want to analyze regions discarted by this filter
    /*if (( sumDistances > yMax ) || ( sumDistances < yMin  ))
    {
        ofstream f;
        f.open("deletedDIAGONAL.txt", ofstream::app);
        f << m_measure.faceNum << endl;
        f.close();
    }*/

    return false;

    MRPT_END
}

//------------------------------------------------------------------------
//                          checkIfDiagonalSurface2
//------------------------------------------------------------------------

bool CFaceDetection::checkIfDiagonalSurface2(CObservation3DRangeScan* face)
{
    MRPT_START

    // To obtain experimental results
    {
        if (m_options.useDiagonalDistanceFilter && m_measure.takeTime)
            m_timeLog.enter("Check if face plane: diagonal distances");

        if (m_options.useSizeDistanceRelationFilter && m_measure.takeTime)
            m_timeLog.enter("Check if face plane: size-distance relation");
    }

    const unsigned int faceWidth = face->intensityImage.getWidth();
    const unsigned int faceHeight = face->intensityImage.getHeight();

    CMatrixDynamic<bool> region;  // To save the segmented region
    experimental_segmentFace(*face, region);

    size_t cont = 0;
    size_t total = 0;
    float sumDepth = 0;

    vector<TPoint3D> points;

    for (unsigned int row = 0; row < faceHeight; row++)
    {
        for (unsigned int col = 0; col < faceWidth; col++, cont++)
        {
            if ((region(row, col)) &&
                (face->confidenceImage.at<uint8_t>(col, row) >
                 m_options.confidenceThreshold))
            {
                sumDepth += face->points3D_x[cont];
                total++;
                points.emplace_back(
                    face->points3D_x[cont], face->points3D_y[cont],
                    face->points3D_z[cont]);
            }
        }
    }

    double meanDepth = sumDepth / total;

    bool res = true;

    if (m_options.useSizeDistanceRelationFilter)
    {
        double maxFaceDistance = 0.5 + 1000 / (pow(faceWidth, 1.9));

        // To obtain experimental results
        {
            if (m_measure.takeTime)
                m_timeLog.leave("Check if face plane: size-distance relation");

            if (m_options.useDiagonalDistanceFilter && m_measure.takeTime)
                m_timeLog.leave("Check if face plane: diagonal distances");
        }

        /*if ( maxFaceDistance > meanDepth )
            return true;

        if ( !m_options.useDiagonalDistanceFilter )
            return false;*/

        if (maxFaceDistance < meanDepth)
        {
            // Uncomment if you want to analyze the regions discarted by this
            // filter
            /*ofstream f;
            f.open("deletedSIZEDISTANCE.txt", ofstream::app);
            f << m_measure.faceNum << endl;
            f.close();*/

            // if ( !m_options.useDiagonalDistanceFilter )
            return false;
            // else
            //  res = false;
        }

        if (!m_options.useDiagonalDistanceFilter) return true;
    }

    ofstream f;
    /*f.open("relaciones1.txt", ofstream::app);
    f << faceWidth << endl;
    f.close();*/

    f.open("relaciones2.txt", ofstream::app);
    f << meanDepth << endl;
    f.close();

    // cout << m_measure.faceNum ;

    // experimental_viewFacePointsScanned( points );

    points.clear();

    /*if (  m_measure.faceNum > 838 )
        experimental_viewFacePointsScanned( points );*/

    // if ( ( m_measure.faceNum == 225 ) || ( m_measure.faceNum == 226 ) )
    // experimental_viewFacePointsScanned( points );

    double sumDistances = 0;
    double distance;
    size_t offsetIndex = 0;

    cont = 0;

    for (unsigned int i = 0; i < faceHeight; i++)
    {
        for (unsigned int j = 0; j < faceWidth; j++, cont++)
        {
            if (region(i, j))
            {
                distance = 0;
                if ((i + 1 < faceHeight) && (j + 1 < faceWidth))
                {
                    if (region(i + 1, j + 1))
                    {
                        TPoint3D p1(
                            face->points3D_x[cont], face->points3D_y[cont],
                            face->points3D_z[cont]);
                        offsetIndex = cont + faceWidth + 1;
                        distance = p1.distanceTo(TPoint3D(
                            face->points3D_x[offsetIndex],
                            face->points3D_y[offsetIndex],
                            face->points3D_z[offsetIndex]));
                    }
                    else
                    {
                        bool validOffset = true;
                        int offset = 2;

                        while (validOffset)
                        {
                            if ((i + offset < faceHeight) &&
                                (j + offset < faceWidth))
                            {
                                if (region(i + offset, j + offset))
                                {
                                    TPoint3D p1(
                                        face->points3D_x[cont],
                                        face->points3D_y[cont],
                                        face->points3D_z[cont]);
                                    offsetIndex = cont + faceWidth + offset;
                                    distance = p1.distanceTo(TPoint3D(
                                        face->points3D_x[offsetIndex],
                                        face->points3D_y[offsetIndex],
                                        face->points3D_z[offsetIndex]));
                                    break;
                                }
                                offset++;
                            }
                            else
                                validOffset = false;
                        }
                    }
                }

                sumDistances += distance;
            }
        }
    }

    // For experimental results
    {
        if (m_measure.takeMeasures)
            m_measure.sumDistances.push_back(sumDistances);

        ofstream fo;
        fo.open("distances.txt", ofstream::app);
        // f << m_measure.faceNum << " " << sumDistances << endl;
        fo << sumDistances << endl;
        fo.close();

        /*f.open("distances2.txt", ofstream::app);
        f << m_measure.faceNum << " " << sumDistances << endl;
        f.close();*/
    }

    // double yMax = 3 + 3.8 / ( pow( meanDepth, 2 ) );
    // double yMax = 3 + 7 /( pow( meanDepth, 2) ) ;
    double yMax = 3 + 11.8 / (pow(meanDepth, 0.9));
    double yMin = 1 + 3.8 / (pow(meanDepth + 7, 6));

    // To obtain experimental results
    {
        if (m_measure.takeTime)
            m_timeLog.leave("Check if face plane: diagonal distances");
    }

    if (((sumDistances <= yMax) && (sumDistances >= yMin)) && (res))
    {
        /* Uncomment if you want to analyze the real size of each studied region
        / *ofstream f;
        f.open("sizes.txt", ofstream::app);
        double h = meanDepth/cos(DEG2RAD(faceHeight*0.2361111111111111));
        double realHigh = sin(DEG2RAD(faceHeight*0.2361111111111111))*h;
        f << realHigh << endl;
        f.close();*/

        return true;
    }

    // Uncomment if you want to analyze regions discarted by this filter
    /*if (( sumDistances > yMax ) || ( sumDistances < yMin  ))
    {
        ofstream f;
        f.open("deletedDIAGONAL.txt", ofstream::app);
        f << m_measure.faceNum << endl;
        f.close();
    }*/

    return false;

    MRPT_END
}

//------------------------------------------------------------------------
//                  experimental_viewFacePointsScanned
//------------------------------------------------------------------------

void CFaceDetection::experimental_viewFacePointsScanned(
    const CObservation3DRangeScan& face)
{
    vector<float> xs, ys, zs;

    unsigned int N = face.points3D_x.size();

    xs.resize(N);
    ys.resize(N);
    zs.resize(N);

    for (unsigned int i = 0; i < N; i++)
    {
        xs[i] = face.points3D_x[i];
        ys[i] = face.points3D_y[i];
        zs[i] = face.points3D_z[i];
    }

    experimental_viewFacePointsScanned(xs, ys, zs);
}

//------------------------------------------------------------------------
//                  experimental_ViewFacePointsScanned
//------------------------------------------------------------------------

void CFaceDetection::experimental_viewFacePointsScanned(
    const vector<TPoint3D>& points)
{
    vector<float> xs, ys, zs;

    unsigned int N = points.size();

    xs.resize(N);
    ys.resize(N);
    zs.resize(N);

    for (unsigned int i = 0; i < N; i++)
    {
        xs[i] = points[i].x;
        ys[i] = points[i].y;
        zs[i] = points[i].z;
    }

    experimental_viewFacePointsScanned(xs, ys, zs);
}

//------------------------------------------------------------------------
//                  experimental_viewFacePointsScanned
//------------------------------------------------------------------------

void CFaceDetection::experimental_viewFacePointsScanned(
    const vector<float>& xs, const vector<float>& ys, const vector<float>& zs)
{
    mrpt::gui::CDisplayWindow3D win3D;

    win3D.setWindowTitle("3D Face detected (Scanned points)");

    win3D.resize(400, 300);

    win3D.setCameraAzimuthDeg(140);
    win3D.setCameraElevationDeg(20);
    win3D.setCameraZoom(6.0);
    win3D.setCameraPointingToPoint(2.5, 0, 0);

    mrpt::opengl::CPointCloudColoured::Ptr gl_points =
        mrpt::opengl::CPointCloudColoured::Create();
    gl_points->setPointSize(4.5);

    mrpt::opengl::COpenGLScene::Ptr scene = win3D.get3DSceneAndLock();

    scene->insert(gl_points);
    scene->insert(mrpt::opengl::CGridPlaneXY::Create());

    CColouredPointsMap pntsMap;

    pntsMap.setAllPoints(xs, ys, zs);

    gl_points->loadFromPointsMap(&pntsMap);

    // gl_points->setColor(0,0.7,0.7,1);

    /*static int i = 0;

    if ( i == 2 )
    {
        mapa.setAllPoints( xs, ys, zs );
        i++;
    }
    else if ( i > 2 )
    {
        float run_time;
        CICP    icp;
        CICP::TReturnInfo   icp_info;

        icp.options.thresholdDist = 0.40;
        icp.options.thresholdAng = 0.40;

        CPose3DPDF::Ptr pdf= icp.Align3D(
        &mapa,    // Map to align
        &pntsMap,          // Reference map
        CPose3D(),    // Initial gross estimate
        &run_time,
        &icp_info);

        cout << "ICP run took " << run_time << " secs." << endl;
        cout << "Goodness: " << 100*icp_info.goodness << "%" << endl;
    }

    i++;*/

    win3D.unlockAccess3DScene();
    win3D.repaint();

    system::pause();
}

//------------------------------------------------------------------------
//              experimental_viewFacePointsAndEigenVects
//------------------------------------------------------------------------

void CFaceDetection::experimental_viewFacePointsAndEigenVects(
    const vector<CVectorFixedDouble<3>>& pointsVector,
    const CMatrixDouble& eigenVect, const std::vector<double>& eigenVal)
{
    vector<float> xs, ys, zs;

    const size_t size = pointsVector.size();

    xs.resize(size);
    ys.resize(size);
    zs.resize(size);

    for (size_t i = 0; i < size; i++)
    {
        xs[i] = pointsVector[i][0];
        ys[i] = pointsVector[i][1];
        zs[i] = pointsVector[i][2];
    }

    TPoint3D center(sum(xs) / size, sum(ys) / size, sum(zs) / size);

    mrpt::gui::CDisplayWindow3D win3D;

    win3D.setWindowTitle("3D Face detected (Scanned points)");

    win3D.resize(400, 300);

    win3D.setCameraAzimuthDeg(140);
    win3D.setCameraElevationDeg(20);
    win3D.setCameraZoom(6.0);
    win3D.setCameraPointingToPoint(2.5, 0, 0);

    mrpt::opengl::CPointCloudColoured::Ptr gl_points =
        mrpt::opengl::CPointCloudColoured::Create();
    gl_points->setPointSize(4.5);

    mrpt::opengl::COpenGLScene::Ptr scene = win3D.get3DSceneAndLock();

    CSphere::Ptr sphere = std::make_shared<CSphere>(0.005f);
    sphere->setLocation(center);
    sphere->setColor(TColorf(0, 1, 0));
    scene->insert(sphere);

    TPoint3D E1(eigenVect(0, 0), eigenVect(0, 1), eigenVect(0, 2));
    TPoint3D E2(eigenVect(1, 0), eigenVect(1, 1), eigenVect(1, 2));
    TPoint3D E3(eigenVect(2, 0), eigenVect(2, 1), eigenVect(2, 2));

    // vector<TSegment3D> sgms;

    TPoint3D p1(center + E1 * eigenVal[0] * 100);
    TPoint3D p2(center + E2 * eigenVal[1] * 100);
    TPoint3D p3(center + E3 * eigenVal[2] * 100);

    CArrow::Ptr arrow1 = std::make_shared<CArrow>(
        center.x, center.y, center.z, p1.x, p1.y, p1.z);
    CArrow::Ptr arrow2 = std::make_shared<CArrow>(
        center.x, center.y, center.z, p2.x, p2.y, p2.z);
    CArrow::Ptr arrow3 = std::make_shared<CArrow>(
        center.x, center.y, center.z, p3.x, p3.y, p3.z);

    arrow1->setColor(TColorf(0, 1, 0));
    arrow2->setColor(TColorf(1, 0, 0));
    arrow3->setColor(TColorf(0, 0, 1));

    scene->insert(arrow1);
    scene->insert(arrow2);
    scene->insert(arrow3);

    // sgms.push_back( TSegment3D(center,center + E1*eigenVal[0]*100) );
    // sgms.push_back( TSegment3D(center,center + E2*eigenVal[1]*100) );
    // sgms.push_back( TSegment3D(center,center + E3*eigenVal[2]*100) );
    // mrpt::opengl::CSetOfLines::Ptr lines =
    // mrpt::opengl::CSetOfLines::Create( sgms );
    // lines->setColor(0,0,1,1);
    // lines->setLineWidth( 10 );

    // scene->insert( lines );

    scene->insert(gl_points);
    scene->insert(mrpt::opengl::CGridPlaneXY::Create());

    CColouredPointsMap pntsMap;

    pntsMap.setAllPoints(xs, ys, zs);

    gl_points->loadFromPointsMap(&pntsMap);

    win3D.unlockAccess3DScene();
    win3D.repaint();

    system::pause();
}

//------------------------------------------------------------------------
//                      experimental_viewRegions
//------------------------------------------------------------------------

void CFaceDetection::experimental_viewRegions(
    const vector<TPoint3D> regions[9], const TPoint3D meanPos[3][3])
{
    mrpt::gui::CDisplayWindow3D win3D;

    win3D.setWindowTitle("3D Face detected (Scanned points)");

    win3D.resize(400, 300);

    win3D.setCameraAzimuthDeg(140);
    win3D.setCameraElevationDeg(20);
    win3D.setCameraZoom(6.0);
    win3D.setCameraPointingToPoint(2.5, 0, 0);

    mrpt::opengl::CPointCloudColoured::Ptr gl_points =
        mrpt::opengl::CPointCloudColoured::Create();
    gl_points->setPointSize(6);

    mrpt::opengl::COpenGLScene::Ptr scene = win3D.get3DSceneAndLock();

    if (meanPos != nullptr)
    {
        for (size_t i = 0; i < 3; i++)
            for (size_t j = 0; j < 3; j++)
            {
                CSphere::Ptr sphere = std::make_shared<CSphere>(0.005f);
                sphere->setLocation(meanPos[i][j]);
                sphere->setColor(TColorf(0, 1, 0));
                scene->insert(sphere);
            }
    }

    vector<TSegment3D> sgms;
    sgms.emplace_back(meanPos[0][0], meanPos[0][1]);
    sgms.emplace_back(meanPos[0][1], meanPos[0][2]);
    sgms.emplace_back(meanPos[1][0], meanPos[1][1]);
    sgms.emplace_back(meanPos[1][1], meanPos[1][2]);
    sgms.emplace_back(meanPos[2][0], meanPos[2][1]);
    sgms.emplace_back(meanPos[2][1], meanPos[2][2]);
    sgms.emplace_back(meanPos[0][0], meanPos[1][1]);
    sgms.emplace_back(meanPos[1][1], meanPos[2][2]);
    sgms.emplace_back(meanPos[2][0], meanPos[1][1]);
    sgms.emplace_back(meanPos[1][1], meanPos[0][2]);
    mrpt::opengl::CSetOfLines::Ptr lines =
        mrpt::opengl::CSetOfLines::Create(sgms);
    lines->setColor(0, 0, 1, 1);
    lines->setLineWidth(10);

    scene->insert(lines);

    scene->insert(gl_points);
    scene->insert(mrpt::opengl::CGridPlaneXY::Create());
    scene->insert(
        mrpt::opengl::CAxis::Create(-5, -5, -5, 5, 5, 5, 2.5, 3, true));

    CColouredPointsMap pntsMap;

    vector<float> xs, ys, zs;

    for (size_t i = 0; i < 9; i++)
        for (const auto& j : regions[i])
        {
            xs.push_back(j.x);
            ys.push_back(j.y);
            zs.push_back(j.z);
        }

    pntsMap.setAllPoints(xs, ys, zs);

    int cont = 0;
    float colors[9][3] = {{1, 0, 0},        {0, 1, 0},        {0, 0, 1},
                          {1, 1, 0},        {1, 0, 1},        {0, 1, 1},
                          {0.5f, 0.25f, 0}, {0.5f, 0, 0.25f}, {0, 0.35f, 0.5f}};
    for (size_t i = 0; i < 9; i++)
    {
        float R = colors[i][0];
        float G = colors[i][1];
        float B = colors[i][2];

        for (unsigned int j = 0; j < regions[i].size(); j++, cont++)
            pntsMap.setPointColor(cont, R, G, B);
    }

    gl_points->loadFromPointsMap(&pntsMap);
    // gl_points->setColorA(0.5);

    win3D.unlockAccess3DScene();
    win3D.repaint();

    system::pause();
}

//------------------------------------------------------------------------
//                      experimental_segmentFace
//------------------------------------------------------------------------

void CFaceDetection::experimental_segmentFace(
    const CObservation3DRangeScan& face, CMatrixDynamic<bool>& region)
{
    const unsigned int faceWidth = face.intensityImage.getWidth();
    const unsigned int faceHeight = face.intensityImage.getHeight();

    region.setSize(faceWidth, faceHeight, true);

    unsigned int x1 = ceil(faceWidth * 0.4);
    unsigned int x2 = floor(faceWidth * 0.6);
    unsigned int y1 = ceil(faceHeight * 0.4);
    unsigned int y2 = floor(faceHeight * 0.6);

    region.setSize(faceHeight, faceWidth);
    CMatrixDynamic<size_t> toExpand;
    toExpand.setSize(faceHeight, faceWidth, true);

    unsigned int cont = (y1 <= 1 ? 0 : faceHeight * (y1 - 1));

    // int total = 0;  // JL: Unused var
    // int numPoints = 0; // JL: Unused var

    mrpt::img::CImage img;
    // Normalize the image
    CMatrixFloat range2D = m_lastFaceDetected.rangeImage;
    range2D *= 1.0f / 5;
    img.setFromMatrix(range2D);

    // INITIALIZATION
    for (unsigned int i = y1; i <= y2; i++)
    {
        cont += x1;

        for (unsigned int j = x1; j <= x2; j++, cont++)
        {
            if (face.confidenceImage.at<uint8_t>(j, i) >
                m_options.confidenceThreshold)
            {
                toExpand(i, j) = 1;
            }
        }
        cont += faceWidth - x2;
    }

    // REGIONS GROWING

    bool newExpanded = true;

    while (newExpanded)
    {
        newExpanded = false;

        for (size_t row = 0; row < faceHeight; row++)
        {
            for (size_t col = 0; col < faceWidth; col++)
            {
                if (toExpand(row, col) == 1)
                {
                    region(row, col) = true;

                    int value = img.at<uint8_t>(col, row);

                    if ((row > 0) && (toExpand(row - 1, col) != 2))
                    {
                        int value2 = img.at<uint8_t>(col, row - 1);
                        if (abs(value - value2) < 2)
                        {
                            toExpand(row - 1, col) = 1;
                            newExpanded = true;
                        }
                    }

                    if ((row < faceWidth - 1) && (toExpand(row + 1, col) != 2))
                    {
                        int value2 = img.at<uint8_t>(col, row + 1);
                        if (abs(value - value2) < 2)
                        {
                            toExpand(row + 1, col) = 1;
                            newExpanded = true;
                        }
                    }

                    if ((col > 0) && (toExpand(row, col - 1) != 2))
                    {
                        int value2 = img.at<uint8_t>(col - 1, row);
                        if (abs(value - value2) < 2)
                        {
                            toExpand(row, col - 1) = 1;
                            newExpanded = true;
                        }
                    }

                    if ((col < faceHeight - 1) && (toExpand(row, col + 1) != 2))
                    {
                        int value2 = img.at<uint8_t>(col + 1, row);
                        if (abs(value - value2) < 2)
                        {
                            toExpand(row, col + 1) = 1;
                            newExpanded = true;
                        }
                    }

                    toExpand(row, col) = 2;
                }
            }
        }
    }

    for (unsigned int row = 0; row < faceHeight; row++)
    {
        for (unsigned int col = 0; col < faceWidth; col++)
        {
            if (!(region(row, col)))
            {
                img.setPixel(col, row, 0);
            }
        }
    }

    // Uncomment if you want to see the resultant region segmented
    if (m_measure.faceNum >= 314)
    {
        CDisplayWindow win("Live video");

        win.showImage(img);
        system::pause();
    }
}

//------------------------------------------------------------------------
//                      experimental_calcHist
//------------------------------------------------------------------------

void CFaceDetection::experimental_calcHist(
    const CImage& face, const size_t& c1, const size_t& r1, const size_t& c2,
    const size_t& r2, CMatrixDynamic<unsigned int>& hist)
{
    TImageSize size;
    face.getSize(size);
    for (size_t row = r1; row <= r2; row++)
        for (size_t col = c1; col <= c2; col++)
        {
            auto value = face.at<uint8_t>(col, row);
            int count = hist(0, value) + 1;
            hist(0, value) = count;
        }
}

//------------------------------------------------------------------------
//                  experimental_showMeasurements
//------------------------------------------------------------------------

void CFaceDetection::experimental_showMeasurements()
{
    // This method execution time is not critical because it's executed only at
    // the end
    // or a few times in user application

    ofstream f;
    f.open("statistics.txt", ofstream::app);

    if (m_measure.lessEigenVals.size() > 0)
    {
        double meanEigenVal, stdEigenVal;
        double minEigenVal = *min_element(
            m_measure.lessEigenVals.begin(), m_measure.lessEigenVals.end());
        double maxEigenVal = *max_element(
            m_measure.lessEigenVals.begin(), m_measure.lessEigenVals.end());

        meanAndStd(m_measure.lessEigenVals, meanEigenVal, stdEigenVal);

        cout << endl
             << "Statistical data about eigen values calculated of regions "
                "detected as faces"
             << endl;
        cout << "Min eigenVal: " << minEigenVal << endl;
        cout << "Max eigenVal: " << maxEigenVal << endl;
        cout << "Mean eigenVal: " << meanEigenVal << endl;
        cout << "Standard Desv: " << stdEigenVal << endl;

        if (m_measure.saveMeasurementsToFile)
        {
            f << endl
              << "Statistical data about eigen values calculated of regions "
                 "detected as faces"
              << endl;
            f << "Min eigenVal: " << minEigenVal << endl;
            f << "Max eigenVal: " << maxEigenVal << endl;
            f << "Mean eigenVal: " << meanEigenVal << endl;
            f << "Standard Desv: " << stdEigenVal << endl;
        }
    }

    if (m_measure.sumDistances.size() > 0)
    {
        double meanSumDist, stdSumDist;
        double minSumDist = *min_element(
            m_measure.sumDistances.begin(), m_measure.sumDistances.end());
        double maxSumDist = *max_element(
            m_measure.sumDistances.begin(), m_measure.sumDistances.end());

        meanAndStd(m_measure.sumDistances, meanSumDist, stdSumDist);

        cout << endl << "Statistical data about sum of distances" << endl;
        cout << "Min sumDistances: " << minSumDist << endl;
        cout << "Max sumDistances: " << maxSumDist << endl;
        cout << "Mean sumDistances: " << meanSumDist << endl;
        cout << "Standard Desv: " << stdSumDist << endl;

        if (m_measure.saveMeasurementsToFile)
        {
            f << endl << "Statistical data about sum of distances" << endl;
            f << "Min sumDistances: " << minSumDist << endl;
            f << "Max sumDistances: " << maxSumDist << endl;
            f << "Mean sumDistances: " << meanSumDist << endl;
            f << "Standard Desv: " << stdSumDist << endl;
        }
    }

    if (m_measure.errorEstimations.size() > 0)
    {
        double meanEstimationErr, stdEstimationErr;
        double minEstimationErr = *min_element(
            m_measure.errorEstimations.begin(),
            m_measure.errorEstimations.end());
        double maxEstimationErr = *max_element(
            m_measure.errorEstimations.begin(),
            m_measure.errorEstimations.end());

        meanAndStd(
            m_measure.errorEstimations, meanEstimationErr, stdEstimationErr);

        cout << endl
             << "Statistical data about estimation error adjusting a plane of "
                "regions detected as faces"
             << endl;
        cout << "Min estimation: " << minEstimationErr << endl;
        cout << "Max estimation: " << maxEstimationErr << endl;
        cout << "Mean estimation: " << meanEstimationErr << endl;
        cout << "Standard Desv: " << stdEstimationErr << endl;

        if (m_measure.saveMeasurementsToFile)
        {
            f << endl
              << "Statistical data about estimation error adjusting a plane of "
                 "regions detected as faces"
              << endl;
            f << "Min estimation: " << minEstimationErr << endl;
            f << "Max estimation: " << maxEstimationErr << endl;
            f << "Mean estimation: " << meanEstimationErr << endl;
            f << "Standard Desv: " << stdEstimationErr << endl;
        }
    }

    cout << endl << "Data about number of faces" << endl;
    cout << "Possible faces detected: " << m_measure.numPossibleFacesDetected
         << endl;
    cout << "Real faces detected: " << m_measure.numRealFacesDetected << endl;

    if (m_meanHist.size() > 0)
    {
        double minHist = *min_element(m_meanHist.begin(), m_meanHist.end());
        double maxHist = *max_element(m_meanHist.begin(), m_meanHist.end());
        double meanHist;
        double stdHist;
        meanAndStd(m_meanHist, meanHist, stdHist);

        cout << endl << "Mean hist: " << meanHist << endl;
        cout << "Min hist: " << minHist << endl;
        cout << "Max hist: " << maxHist << endl;
        cout << "Stdv: " << stdHist << endl;
    }

    if (m_measure.saveMeasurementsToFile)
    {
        f << endl << "Data about number of faces" << endl;
        f << "Possible faces detected: " << m_measure.numPossibleFacesDetected
          << endl;
        f << "Real faces detected: " << m_measure.numRealFacesDetected << endl;
    }

    if (m_measure.takeTime && m_measure.saveMeasurementsToFile)
        f << endl << m_timeLog.getStatsAsText();

    f.close();

    mrpt::system::pause();
}

//------------------------------------------------------------------------
//                      debug_returnResults
//------------------------------------------------------------------------

void CFaceDetection::debug_returnResults(
    const std::vector<uint32_t>& falsePositives,
    const std::vector<uint32_t>& ignore, unsigned int& falsePositivesDeleted,
    unsigned int& realFacesDeleted)
{
    const unsigned int numDeleted = m_measure.deletedRegions.size();
    const unsigned int numFalsePositives = falsePositives.size();
    const unsigned int numIgnored = ignore.size();
    unsigned int ignoredDetected = 0;

    falsePositivesDeleted = 0;

    for (unsigned int i = 0; i < numDeleted; i++)
    {
        unsigned int region = m_measure.deletedRegions[i];

        bool falsePositive = false;

        unsigned int j = 0;
        while (!falsePositive && (j < numFalsePositives))
        {
            if (region == falsePositives[j]) falsePositive = true;
            j++;
        }

        if (falsePositive)
            falsePositivesDeleted++;
        else
        {
            bool igno = false;

            j = 0;
            while (!igno && (j < numIgnored))
            {
                if (region == ignore[j]) igno = true;
                j++;
            }

            if (igno) ignoredDetected++;
        }
    }

    realFacesDeleted = numDeleted - falsePositivesDeleted - ignoredDetected;

    m_measure.faceNum = 0;
    m_measure.deletedRegions.clear();
}