Plane.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          |
   +------------------------------------------------------------------------+ */

/*  Plane-based Map (PbMap) library
 *  Construction of plane-based maps and localization in it from RGBD Images.
 *  Writen by Eduardo Fernandez-Moral. See docs for <a
 * href="group__mrpt__pbmap__grp.html" >mrpt-pbmap</a>
 */

#include "pbmap-precomp.h"  // Precompiled headers

#if MRPT_HAS_PCL

#include <mrpt/pbmap/Miscellaneous.h>
#include <mrpt/pbmap/Plane.h>
#include <pcl/common/time.h>
#include <pcl/filters/voxel_grid.h>
#include <cassert>

using namespace mrpt::pbmap;
using namespace std;

IMPLEMENTS_SERIALIZABLE(Plane, CSerializable, mrpt::pbmap)

///*---------------------------------------------------------------
//                      writeToStream
// ---------------------------------------------------------------*/
// uint8_t  Plane::serializeGetVersion() const { return XX; } void
// Plane::serializeTo(mrpt::serialization::CArchive& out) const
//{
//  if (out_Version)
//      *out_Version = 0;
//  else
//  {
// cout << "write plane \n";
//      // The data
//      out << static_cast<uint32_t>(numObservations);//out <<
// uint32_t(numObservations);
//      out << areaVoxels;
//      out << areaHull;
//      out << elongation;
//    out << v3normal(0) << v3normal(1) << v3normal(2);
//    out << v3center(0) << v3center(1) << v3center(2);
//    out << v3PpalDir(0) << v3PpalDir(1) << v3PpalDir(2);
//    out << v3colorNrgb(0) << v3colorNrgb(1) << v3colorNrgb(2);
//    out << v3colorNrgbDev(0) << v3colorNrgbDev(1) << v3colorNrgbDev(2);
////    out.WriteBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3normal(0),3);
////    out.WriteBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3center(0),3);
////    out.WriteBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3PpalDir(0),3);
//// out.WriteBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3colorNrgb(0),3);
////
/// out.WriteBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3colorNrgbDev(0),3);
//
//    out << (uint32_t)neighborPlanes.size();
//    for (std::map<unsigned,unsigned>::const_iterator
//    it=neighborPlanes.begin(); it != neighborPlanes.end(); it++)
//      out << static_cast<uint32_t>(it->first) <<
//      static_cast<uint32_t>(it->second);
//
//    out << (uint32_t)polygonContourPtr->size();
//    for (uint32_t i=0; i < polygonContourPtr->size(); i++)
//      out << polygonContourPtr->points[i].x << polygonContourPtr->points[i].y
//      << polygonContourPtr->points[i].z;
//
//    out << bFullExtent;
//    out << bFromStructure;
//  }
//
//}
//
///*---------------------------------------------------------------
//                      readFromStream
// ---------------------------------------------------------------*/
// void  Plane::serializeFrom(mrpt::serialization::CArchive& in, uint8_t
// version)
//{
//  switch(version)
//  {
//  case 0:
//      {
//      cout << "Read plane\n";
//
//          // The data
//          uint32_t n;
//          in >> n;
//          numObservations = (unsigned)n;
//          in >> areaVoxels;
//          in >> areaHull;
//          in >> elongation;
//          in >> v3normal(0) >> v3normal(1) >> v3normal(2);
//          in >> v3center(0) >> v3center(1) >> v3center(2);
//          in >> v3PpalDir(0) >> v3PpalDir(1) >> v3PpalDir(2);
//          in >> v3colorNrgb(0) >> v3colorNrgb(1) >> v3colorNrgb(2);
//          in >> v3colorNrgbDev(0) >> v3colorNrgbDev(1) >> v3colorNrgbDev(2);
////
/// in.ReadBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3normal[0],sizeof(v3normal[0])*3);
////
/// in.ReadBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3center[0],sizeof(v3center[0])*3);
////
/// in.ReadBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3PpalDir[0],sizeof(v3PpalDir[0])*3);
////
/// in.ReadBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3colorNrgb[0],sizeof(v3colorNrgb[0])*3);
////
/// in.ReadBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3colorNrgbDev[0],sizeof(v3colorNrgbDev[0])*3);
//
//          in >> n;
//          neighborPlanes.clear();
//      for (uint32_t i=0; i < n; i++)
//      {
//        uint32_t neighbor, commonObs;
//        in >> neighbor >> commonObs;
//        neighborPlanes[neighbor] = commonObs;
//      }
//
//          in >> n;
//          polygonContourPtr->resize(n);
//      for (unsigned i=0; i < n; i++)
//        in >> polygonContourPtr->points[i].x >> polygonContourPtr->points[i].y
//        >> polygonContourPtr->points[i].z;
//
//      in >> bFullExtent;
//      in >> bFromStructure;
//
//      } break;
//  default:
//      MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
//  };
//}

uint8_t Plane::serializeGetVersion() const { return 0; }
void Plane::serializeTo(mrpt::serialization::CArchive& out) const
{
    // The data
    //      out << static_cast<uint32_t>(numObservations);//out <<
    // uint32_t(numObservations);
    //      out << areaVoxels;
    out << areaHull;
    out << elongation;
    out << curvature;
    out << v3normal(0) << v3normal(1) << v3normal(2);
    out << v3center(0) << v3center(1) << v3center(2);
    out << v3PpalDir(0) << v3PpalDir(1) << v3PpalDir(2);
    out << v3colorNrgb(0) << v3colorNrgb(1) << v3colorNrgb(2);
    ////    out << v3colorNrgbDev(0) << v3colorNrgbDev(1) <<
    /// v3colorNrgbDev(2);
    out << dominantIntensity;
    out << bDominantColor;

    out << hist_H;
    // cout << "color " << hist_H.size() << endl;
    //    for (size_t i=0; i < 74; i++){//cout << hist_H[i] << " ";
    //      out << hist_H[i];}

    out << inliers;

    out << label;
    out << label_object;
    out << label_context;

    out << (uint32_t)polygonContourPtr->size();
    for (uint32_t i = 0; i < polygonContourPtr->size(); i++)
        out << polygonContourPtr->points[i].x << polygonContourPtr->points[i].y
            << polygonContourPtr->points[i].z;
}

void Plane::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
{
    switch (version)
    {
        case 0:
        {
            //          // The data
            uint32_t n;
            //          in >> n;
            //          numObservations = (unsigned)n;
            //          in >> areaVoxels;
            in >> areaHull;
            in >> elongation;
            in >> curvature;
            in >> v3normal(0) >> v3normal(1) >> v3normal(2);
            in >> v3center(0) >> v3center(1) >> v3center(2);
            in >> v3PpalDir(0) >> v3PpalDir(1) >> v3PpalDir(2);
            d = -v3normal.dot(v3center);
            in >> v3colorNrgb(0) >> v3colorNrgb(1) >> v3colorNrgb(2);
            ////            in >> v3colorNrgbDev(0) >> v3colorNrgbDev(1) >>
            /// v3colorNrgbDev(2);
            in >> dominantIntensity;
            in >> bDominantColor;

            in >> hist_H;

            in >> inliers;

            in >> label;
            in >> label_object;
            in >> label_context;

            in >> n;
            polygonContourPtr->resize(n);
            for (unsigned i = 0; i < n; i++)
                in >> polygonContourPtr->points[i].x >>
                    polygonContourPtr->points[i].y >>
                    polygonContourPtr->points[i].z;
        }
        break;
        default:
            MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
    };
}

/*!
 * Force the plane inliers to lay on the plane
 */
void Plane::forcePtsLayOnPlane()
{
    // The plane equation has the form Ax + By + Cz + D = 0, where the vector
    // N=(A,B,C) is the normal and the constant D can be calculated as D =
    // -N*(PlanePoint) = -N*PlaneCenter
    const double D = -(v3normal.dot(v3center));
    for (unsigned i = 0; i < planePointCloudPtr->size(); i++)
    {
        double dist = v3normal[0] * planePointCloudPtr->points[i].x +
                      v3normal[1] * planePointCloudPtr->points[i].y +
                      v3normal[2] * planePointCloudPtr->points[i].z + D;
        planePointCloudPtr->points[i].x -= v3normal[0] * dist;
        planePointCloudPtr->points[i].y -= v3normal[1] * dist;
        planePointCloudPtr->points[i].z -= v3normal[2] * dist;
    }
    // Do the same with the points defining the convex hull
    for (unsigned i = 0; i < polygonContourPtr->size(); i++)
    {
        double dist = v3normal[0] * polygonContourPtr->points[i].x +
                      v3normal[1] * polygonContourPtr->points[i].y +
                      v3normal[2] * polygonContourPtr->points[i].z + D;
        polygonContourPtr->points[i].x -= v3normal[0] * dist;
        polygonContourPtr->points[i].y -= v3normal[1] * dist;
        polygonContourPtr->points[i].z -= v3normal[2] * dist;
    }
}

/** \brief Compute the area of a 2D planar polygon patch
 */
float Plane::compute2DPolygonalArea()
{
    int k0, k1, k2;

    // Find axis with largest normal component and project onto perpendicular
    // plane
    k0 = (fabs(v3normal[0]) > fabs(v3normal[1])) ? 0 : 1;
    k0 = (fabs(v3normal[k0]) > fabs(v3normal[2])) ? k0 : 2;
    k1 = (k0 + 1) % 3;
    k2 = (k0 + 2) % 3;

    // cos(theta), where theta is the angle between the polygon and the
    // projected plane
    float ct = fabs(v3normal[k0]);
    float AreaX2 = 0.0;
    float p_i[3], p_j[3];

    for (unsigned int i = 0; i < polygonContourPtr->points.size(); i++)
    {
        p_i[0] = polygonContourPtr->points[i].x;
        p_i[1] = polygonContourPtr->points[i].y;
        p_i[2] = polygonContourPtr->points[i].z;
        int j = (i + 1) % polygonContourPtr->points.size();
        p_j[0] = polygonContourPtr->points[j].x;
        p_j[1] = polygonContourPtr->points[j].y;
        p_j[2] = polygonContourPtr->points[j].z;

        AreaX2 += p_i[k1] * p_j[k2] - p_i[k2] * p_j[k1];
    }
    AreaX2 = fabs(AreaX2) / (2 * ct);

    return AreaX2;
}

/** \brief Compute the patch's convex-hull area and mass center
 */
void Plane::computeMassCenterAndArea()
{
    int k0, k1, k2;

    // Find axis with largest normal component and project onto perpendicular
    // plane
    k0 = (fabs(v3normal[0]) > fabs(v3normal[1])) ? 0 : 1;
    k0 = (fabs(v3normal[k0]) > fabs(v3normal[2])) ? k0 : 2;
    k1 = (k0 + 1) % 3;
    k2 = (k0 + 2) % 3;

    // cos(theta), where theta is the angle between the polygon and the
    // projected plane
    float ct = fabs(v3normal[k0]);
    float AreaX2 = 0.0;
    Eigen::Vector3f massCenter = Eigen::Vector3f::Zero();
    float p_i[3], p_j[3];

    for (unsigned int i = 0; i < polygonContourPtr->points.size(); i++)
    {
        p_i[0] = polygonContourPtr->points[i].x;
        p_i[1] = polygonContourPtr->points[i].y;
        p_i[2] = polygonContourPtr->points[i].z;
        int j = (i + 1) % polygonContourPtr->points.size();
        p_j[0] = polygonContourPtr->points[j].x;
        p_j[1] = polygonContourPtr->points[j].y;
        p_j[2] = polygonContourPtr->points[j].z;
        double cross_segment = p_i[k1] * p_j[k2] - p_i[k2] * p_j[k1];

        AreaX2 += cross_segment;
        massCenter[k1] += (p_i[k1] + p_j[k1]) * cross_segment;
        massCenter[k2] += (p_i[k2] + p_j[k2]) * cross_segment;
    }
    areaHull = fabs(AreaX2) / (2 * ct);

    massCenter[k1] /= (3 * AreaX2);
    massCenter[k2] /= (3 * AreaX2);
    massCenter[k0] = (v3normal.dot(v3center) - v3normal[k1] * massCenter[k1] -
                      v3normal[k2] * massCenter[k2]) /
                     v3normal[k0];

    v3center = massCenter;

    d = -(v3normal.dot(v3center));
}

void Plane::calcElongationAndPpalDir()
{
    pcl::PCA<PointT> pca;
    pca.setInputCloud(planePointCloudPtr);
    Eigen::VectorXf eigenVal = pca.getEigenValues();
    //  if( eigenVal[0] > 2 * eigenVal[1] )
    {
        elongation = sqrt(eigenVal[0] / eigenVal[1]);
        Eigen::MatrixXf eigenVect = pca.getEigenVectors();
        //    v3PpalDir = makeVector(eigenVect(0,0), eigenVect(1,0),
        //    eigenVect(2,0));
        v3PpalDir[0] = eigenVect(0, 0);
        v3PpalDir[1] = eigenVect(1, 0);
        v3PpalDir[2] = eigenVect(2, 0);
    }
}

// The point cloud of the plane must be filled before using this function
void Plane::getPlaneNrgb()
{
    // cout << "Plane::getPlaneNrgb() " << planePointCloudPtr->size() << endl;
    assert(planePointCloudPtr->size() > 0);

    r.resize(planePointCloudPtr->size());
    g.resize(planePointCloudPtr->size());
    b.resize(planePointCloudPtr->size());
    intensity.resize(planePointCloudPtr->size());

    size_t countPix = 0;
    for (size_t i = 0; i < planePointCloudPtr->size(); i++)
    {
        float sumRGB = (float)planePointCloudPtr->points[i].r +
                       planePointCloudPtr->points[i].g +
                       planePointCloudPtr->points[i].b;
        intensity[i] = sumRGB;
        if (sumRGB != 0)
        {
            r[countPix] = planePointCloudPtr->points[i].r / sumRGB;
            g[countPix] = planePointCloudPtr->points[i].g / sumRGB;
            b[countPix] = planePointCloudPtr->points[i].b / sumRGB;
            ++countPix;
        }
    }
    r.resize(countPix);
    g.resize(countPix);
    b.resize(countPix);
    intensity.resize(countPix);
}

void Plane::getPlaneC1C2C3()
{
    c1.resize(planePointCloudPtr->size());
    c2.resize(planePointCloudPtr->size());
    c3.resize(planePointCloudPtr->size());

    for (unsigned i = 0; i < planePointCloudPtr->size(); i++)
    {
        c1[i] = atan2(
            (double)planePointCloudPtr->points[i].r,
            (double)max(
                planePointCloudPtr->points[i].g,
                planePointCloudPtr->points[i].b));
        c2[i] = atan2(
            (double)planePointCloudPtr->points[i].g,
            (double)max(
                planePointCloudPtr->points[i].r,
                planePointCloudPtr->points[i].b));
        c3[i] = atan2(
            (double)planePointCloudPtr->points[i].b,
            (double)max(
                planePointCloudPtr->points[i].r,
                planePointCloudPtr->points[i].g));
    }
}

void Plane::calcPlaneHistH()
{
    // cout << "Plane::calcPlaneHistH()\n";
    float fR, fG, fB;
    float fH, fS, fV;
    const float FLOAT_TO_BYTE = 255.0f;
    const float BYTE_TO_FLOAT = 1.0f / FLOAT_TO_BYTE;

    vector<int> H(74, 0);

    for (unsigned i = 0; i < planePointCloudPtr->size(); i++)
    {
        // Convert from 8-bit integers to floats.
        fR = planePointCloudPtr->points[i].r * BYTE_TO_FLOAT;
        fG = planePointCloudPtr->points[i].g * BYTE_TO_FLOAT;
        fB = planePointCloudPtr->points[i].b * BYTE_TO_FLOAT;
        // Convert from RGB to HSV, using float ranges 0.0 to 1.0.
        float fDelta;
        float fMin, fMax;
        int iMax;
        // Get the min and max, but use integer comparisons for slight speedup.
        if (planePointCloudPtr->points[i].b < planePointCloudPtr->points[i].g)
        {
            if (planePointCloudPtr->points[i].b <
                planePointCloudPtr->points[i].r)
            {
                fMin = fB;
                if (planePointCloudPtr->points[i].r >
                    planePointCloudPtr->points[i].g)
                {
                    iMax = planePointCloudPtr->points[i].r;
                    fMax = fR;
                }
                else
                {
                    iMax = planePointCloudPtr->points[i].g;
                    fMax = fG;
                }
            }
            else
            {
                fMin = fR;
                fMax = fG;
                iMax = planePointCloudPtr->points[i].g;
            }
        }
        else
        {
            if (planePointCloudPtr->points[i].g <
                planePointCloudPtr->points[i].r)
            {
                fMin = fG;
                if (planePointCloudPtr->points[i].b >
                    planePointCloudPtr->points[i].r)
                {
                    fMax = fB;
                    iMax = planePointCloudPtr->points[i].b;
                }
                else
                {
                    fMax = fR;
                    iMax = planePointCloudPtr->points[i].r;
                }
            }
            else
            {
                fMin = fR;
                fMax = fB;
                iMax = planePointCloudPtr->points[i].b;
            }
        }
        fDelta = fMax - fMin;
        fV = fMax;  // Value (Brightness).
        if (fV < 0.01)
        {
            //      fH = 72; // Histogram has 72 bins for Hue values, and 2 bins
            //      more for black and white
            H[72]++;
            continue;
        }
        else
        //    if (iMax != 0)
        {  // Make sure its not pure black.
            fS = fDelta / fMax;  // Saturation.
            if (fS < 0.2)
            {
                //        fH = 73; // Histogram has 72 bins for Hue values, and
                //        2 bins more for black and white
                H[73]++;
                continue;
            }

            //      float ANGLE_TO_UNIT = 1.0f / (6.0f * fDelta);   // Make the
            //      Hues between 0.0 to 1.0 instead of 6.0
            float ANGLE_TO_UNIT = 12.0f / fDelta;  // 12 bins
            if (iMax == planePointCloudPtr->points[i].r)
            {  // between yellow and magenta.
                fH = (fG - fB) * ANGLE_TO_UNIT;
            }
            else if (iMax == planePointCloudPtr->points[i].g)
            {  // between cyan and yellow.
                fH = (2.0f / 6.0f) + (fB - fR) * ANGLE_TO_UNIT;
            }
            else
            {  // between magenta and cyan.
                fH = (4.0f / 6.0f) + (fR - fG) * ANGLE_TO_UNIT;
            }
            // Wrap outlier Hues around the circle.
            if (fH < 0.0f) fH += 72.0f;
            if (fH >= 72.0f) fH -= 72.0f;
        }
        //  cout << "H " << fH << endl;
        H[int(fH)]++;
    }
    // cout << "FInish histogram calculation\n";
    // Normalize histogram
    float numPixels = 0;
    for (unsigned i = 0; i < H.size(); i++) numPixels += H[i];
    hist_H.resize(74);
    for (unsigned i = 0; i < H.size(); i++) hist_H[i] = H[i] / numPixels;

    // cout << "Got H histogram" << hist_H.size() << "\n";
}

float concentrationThreshold = 0.5;

void Plane::calcMainColor2()
{
    assert(planePointCloudPtr->size() > 0);

    //  double getColorStart, getColorEnd;
    //  getColorStart = pcl::getTime();
    std::vector<Eigen::Vector4f> color(planePointCloudPtr->size());

    size_t countPix = 0;
    size_t stepColor = std::max(
        planePointCloudPtr->size() / 2000,
        static_cast<size_t>(
            1));  // Limit the main color calculation to 2000 pixels
    for (size_t i = 0; i < planePointCloudPtr->size(); i += stepColor)
    {
        float sumRGB = (float)planePointCloudPtr->points[i].r +
                       planePointCloudPtr->points[i].g +
                       planePointCloudPtr->points[i].b;
        if (sumRGB != 0)
        {
            color[countPix][0] = planePointCloudPtr->points[i].r / sumRGB;
            color[countPix][1] = planePointCloudPtr->points[i].g / sumRGB;
            color[countPix][2] = planePointCloudPtr->points[i].b / sumRGB;
            color[countPix][3] = sumRGB;
            //      intensity[countPix] = sumRGB;
            // cout << "color " << color[countPix][0] << " " <<
            // color[countPix][1] << " " << color[countPix][2] << " " <<
            // color[countPix][3] << endl;
            ++countPix;
        }
    }
    color.resize(countPix);
    intensity.resize(countPix);

    float concentration, histStdDev;
    //  Eigen::Vector4f dominantColor;
    Eigen::Vector4f dominantColor =
        getMultiDimMeanShift_color(color, concentration, histStdDev);
    v3colorNrgb = dominantColor.head(3);
    dominantIntensity = dominantColor(3);

    if (concentration > concentrationThreshold)
        bDominantColor = true;
    else
        bDominantColor = false;
    // getColorEnd = pcl::getTime ();
    // cout << "Nrgb2 in " << (getColorEnd - getColorStart)*1000000 << " us\n";
    // cout << "colorNrgb " << v3colorNrgb[0] << " " << v3colorNrgb[1] << " " <<
    // v3colorNrgb[2] << " " << v3colorNrgb[3] << endl;

    // getColorStart = pcl::getTime ();
    //
    //  // c1c2c3
    //  getPlaneC1C2C3();
    //  v3colorC1C2C3[0] = getHistogramMeanShift(c1, (float)1.5708,
    //  v3colorNrgbDev[0]);
    //  v3colorC1C2C3[1] = getHistogramMeanShift(c2, (float)1.5708,
    //  v3colorNrgbDev[1]);
    //  v3colorC1C2C3[2] = getHistogramMeanShift(c3, (float)1.5708,
    //  v3colorNrgbDev[2]);
    // getColorEnd = pcl::getTime ();
    // cout << "c1c2c3 in " << (getColorEnd - getColorStart)*1000000 << " us\n";
    //
    // getColorStart = pcl::getTime ();
    //
    //  // H histogram
    //  calcPlaneHistH();
    // getColorEnd = pcl::getTime ();
    // cout << "HistH in " << (getColorEnd - getColorStart)*1000000 << " us\n";
}

void Plane::calcMainColor()
{
    // Normalized rgb
    // double getColorStart ,getColorEnd;
    // getColorStart = pcl::getTime ();

    getPlaneNrgb();
    // cout << "r size " << r.size() << " " << v3colorNrgbDev(0) << endl;
    v3colorNrgb(0) = getHistogramMeanShift(r, 1.0, v3colorNrgbDev(0));
    v3colorNrgb(1) = getHistogramMeanShift(g, 1.0, v3colorNrgbDev(1));
    v3colorNrgb(2) = getHistogramMeanShift(b, 1.0, v3colorNrgbDev(2));
    ////  dominantIntensity = getHistogramMeanShift(intensity, 767.0,
    /// v3colorNrgbDev(2));
    // assert(v3colorNrgb(0) != 0 && v3colorNrgb(1) != 0 && v3colorNrgb(2) !=
    // 0);
    //
    dominantIntensity = 0;
    int countFringe05 = 0;
    for (unsigned i = 0; i < r.size(); i++)
        if (fabs(r[i] - v3colorNrgb(0)) < 0.05 &&
            fabs(g[i] - v3colorNrgb(1)) < 0.05 &&
            fabs(b[i] - v3colorNrgb(2)) < 0.05)
        {
            dominantIntensity += intensity[i];
            ++countFringe05;
        }
    assert(countFringe05 > 0);
    dominantIntensity /= countFringe05;
    float concentration05 = static_cast<float>(countFringe05) / r.size();

    // getColorEnd = pcl::getTime ();
    // cout << "Nrgb in " << (getColorEnd - getColorStart)*1000000 << " us\n";
    // cout << "Concentration " << concentration1 << " " << concentration2 << "
    // " << concentration3 << endl;
    // We are storing the concentration vale in v3colorNrgbDev to heck if there
    // exists a dominant color
    //  if(concentration1 > concentrationThreshold && concentration2 >
    //  concentrationThreshold && concentration3 > concentrationThreshold)
    if (concentration05 > 0.5)
        bDominantColor = true;
    else
        bDominantColor = false;

    // getColorStart = pcl::getTime ();

    //  // c1c2c3
    //  getPlaneC1C2C3();
    //  v3colorC1C2C3[0] = getHistogramMeanShift(c1, (float)1.5708,
    //  v3colorNrgbDev[0]);
    //  v3colorC1C2C3[1] = getHistogramMeanShift(c2, (float)1.5708,
    //  v3colorNrgbDev[1]);
    //  v3colorC1C2C3[2] = getHistogramMeanShift(c3, (float)1.5708,
    //  v3colorNrgbDev[2]);
    ////getColorEnd = pcl::getTime ();
    ////cout << "c1c2c3 in " << (getColorEnd - getColorStart)*1000000 << "
    /// us\n";
    //
    ////getColorStart = pcl::getTime ();
    // H histogram
    calcPlaneHistH();
    ////cout << "Update color fin" << endl;
    // getColorEnd = pcl::getTime ();
    // cout << "HistH in " << (getColorEnd - getColorStart)*1000000 << " us\n";
}

/**!
 * mPointHull serves to calculate the convex hull of a set of points in 2D,
 * which are defined by its position (x,y)
 * and an identity id
 */
struct mPointHull
{
    float x, y;
    size_t id;

    bool operator<(const mPointHull& p) const
    {
        return x < p.x || (x == p.x && y < p.y);
    }
};

float cross(const mPointHull& O, const mPointHull& A, const mPointHull& B)
{
    return (A.x - O.x) * (B.y - O.y) - (A.y - O.y) * (B.x - O.x);
}

///**!
// * Calculate the plane's convex hull with the monotone chain algorithm.
//*/
// void Plane::calcConvexHull(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr
// &pointCloud)
//{
//  // Find axis with largest normal component and project onto perpendicular
//  plane
//  int k0;//, k1, k2;
//  k0 = (fabs (v3normal(0) ) > fabs(v3normal[1])) ? 0  : 1;
//  k0 = (fabs (v3normal(k0) ) > fabs(v3normal(2))) ? k0 : 2;
//
//  std::vector<mPointHull> P;//(pointCloud->size() );
//  P.resize(pointCloud->size() );
//  if(k0 == 0)
//    for(size_t i=0; i < pointCloud->size(); i++)
//    {
//      P[i].x = pointCloud->points[i].y;
//      P[i].y = pointCloud->points[i].z;
//      P[i].id = i;
//    }
//  else if(k0 == 1)
//    for(size_t i=0; i < pointCloud->size(); i++)
//    {
//      P[i].x = pointCloud->points[i].x;
//      P[i].y = pointCloud->points[i].z;
//      P[i].id = i;
//    }
//  else // (k0 == 2)
//    for(size_t i=0; i < pointCloud->size(); i++)
//    {
//      P[i].x = pointCloud->points[i].x;
//      P[i].y = pointCloud->points[i].y;
//      P[i].id = i;
//    }
//
//  int n = P.size(), k = 0;
//  std::vector<mPointHull> H(2*n);
//
//  // Sort points lexicographically
//  std::sort(P.begin(), P.end());
//
//  // Build lower hull
//  for (int i = 0; i < n; i++)
//  {
//    while (k >= 2 && cross(H[k-2], H[k-1], P[i]) <= 0)
//      k--;
//    H[k++] = P[i];
//    if(k > 0)
//        assert(H[k-1].id != H[k-2].id);
//  }
//
//  // Build upper hull
//  for (int i = n-2, t = k+1; i >= 0; i--)
//  {
//    while (k >= t && cross(H[k-2], H[k-1], P[i]) <= 0)
//      k--;
//    H[k++] = P[i];
//  }
//
//  // Fill convexHull vector
//  size_t hull_noRep = k-1; // Neglect the last_point = first_point
//  H.resize(k);
//  polygonContourPtr->resize(hull_noRep);
//
//  for(size_t i=0; i < hull_noRep; i++)
//  {
//    polygonContourPtr->points[i] = pointCloud->points[ H[i].id ];
//  }
//
//}

void Plane::calcConvexHull(
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr& pointCloud,
    std::vector<size_t>& indices)
{
    // Find axis with largest normal component and project onto perpendicular
    // plane
    int k0;  //, k1, k2;
    k0 = (fabs(v3normal(0)) > fabs(v3normal[1])) ? 0 : 1;
    k0 = (fabs(v3normal(k0)) > fabs(v3normal(2))) ? k0 : 2;

    std::vector<mPointHull> P;  //(pointCloud->size() );
    P.resize(pointCloud->size());
    if (k0 == 0)
        for (size_t i = 0; i < pointCloud->size(); i++)
        {
            P[i].x = pointCloud->points[i].y;
            P[i].y = pointCloud->points[i].z;
            P[i].id = i;
        }
    else if (k0 == 1)
        for (size_t i = 0; i < pointCloud->size(); i++)
        {
            P[i].x = pointCloud->points[i].x;
            P[i].y = pointCloud->points[i].z;
            P[i].id = i;
        }
    else  // (k0 == 2)
        for (size_t i = 0; i < pointCloud->size(); i++)
        {
            P[i].x = pointCloud->points[i].x;
            P[i].y = pointCloud->points[i].y;
            P[i].id = i;
        }

    int n = P.size(), k = 0;
    std::vector<mPointHull> H(2 * n);

    // Sort points lexicographically
    std::sort(P.begin(), P.end());

    // Build lower hull
    for (int i = 0; i < n; i++)
    {
        while (k >= 2 && cross(H[k - 2], H[k - 1], P[i]) <= 0) k--;
        H[k++] = P[i];
        if (k > 0) assert(H[k - 1].id != H[k - 2].id);
    }

    // Build upper hull
    for (int i = n - 2, t = k + 1; i >= 0; i--)
    {
        while (k >= t && cross(H[k - 2], H[k - 1], P[i]) <= 0) k--;
        H[k++] = P[i];
    }

    // Fill convexHull vector (polygonContourPtr)
    size_t hull_noRep = k - 1;  // Neglect the last_point = first_point
    H.resize(k);
    polygonContourPtr->resize(hull_noRep);
    indices.resize(hull_noRep);

    for (size_t i = 0; i < hull_noRep; i++)
    {
        polygonContourPtr->points[i] = pointCloud->points[H[i].id];
        indices[i] = H[i].id;
    }
}

void Plane::calcConvexHullandParams(
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr& pointCloud,
    std::vector<size_t>& indices)
{
    // Find axis with largest normal component and project onto perpendicular
    // plane
    int k0, k1, k2;
    k0 = (fabs(v3normal(0)) > fabs(v3normal[1])) ? 0 : 1;
    k0 = (fabs(v3normal(k0)) > fabs(v3normal(2))) ? k0 : 2;
    k1 = (k0 + 1) % 3;
    k2 = (k0 + 2) % 3;

    std::vector<mPointHull> P;  //(pointCloud->size() );
    P.resize(pointCloud->size());
    if (k0 == 0)
        for (size_t i = 0; i < pointCloud->size(); i++)
        {
            P[i].x = pointCloud->points[i].y;
            P[i].y = pointCloud->points[i].z;
            P[i].id = i;
        }
    else if (k0 == 1)
        for (size_t i = 0; i < pointCloud->size(); i++)
        {
            P[i].x = pointCloud->points[i].z;
            P[i].y = pointCloud->points[i].x;
            P[i].id = i;
        }
    else  // (k0 == 2)
        for (size_t i = 0; i < pointCloud->size(); i++)
        {
            P[i].x = pointCloud->points[i].x;
            P[i].y = pointCloud->points[i].y;
            P[i].id = i;
        }

    int n = P.size(), k = 0;
    std::vector<mPointHull> H(2 * n);

    // Sort points lexicographically
    std::sort(P.begin(), P.end());

    // Build lower hull
    for (int i = 0; i < n; i++)
    {
        while (k >= 2 && cross(H[k - 2], H[k - 1], P[i]) <= 0) k--;
        H[k++] = P[i];
        if (k > 0) assert(H[k - 1].id != H[k - 2].id);
    }

    // Build upper hull
    for (int i = n - 2, t = k + 1; i >= 0; i--)
    {
        while (k >= t && cross(H[k - 2], H[k - 1], P[i]) <= 0) k--;
        H[k++] = P[i];
    }

    // Fill convexHull vector (polygonContourPtr)
    size_t hull_noRep = k - 1;  // Neglect the last_point = first_point
    H.resize(k);
    polygonContourPtr->resize(hull_noRep);
    indices.resize(hull_noRep);

    for (size_t i = 0; i < hull_noRep; i++)
    {
        polygonContourPtr->points[i] = pointCloud->points[H[i].id];
        indices[i] = H[i].id;
    }

    // Calc area and mass center
    float ct = fabs(v3normal[k0]);
    float AreaX2 = 0.0;
    Eigen::Vector3f massCenter = Eigen::Vector3f::Zero();
    for (size_t i = 0, j = 1; i < hull_noRep; i++, j++)
    {
        float cross_segment = H[i].x * H[j].y - H[i].y * H[j].x;

        AreaX2 += cross_segment;
        massCenter[k1] += (H[i].x + H[j].x) * cross_segment;
        massCenter[k2] += (H[i].y + H[j].y) * cross_segment;
    }
    areaHull = fabs(AreaX2) / (2 * ct);

    v3center[k1] /= (3 * AreaX2);
    v3center[k2] /= (3 * AreaX2);
    v3center[k0] =
        (-d - v3normal[k1] * massCenter[k1] - v3normal[k2] * massCenter[k2]) /
        v3normal[k0];

    d = -v3normal.dot(v3center);

    // Compute elongation and ppal direction
    Eigen::Matrix2f covariances = Eigen::Matrix2f::Zero();
    Eigen::Vector2f p1, p2;
    p2[0] = H[0].x - massCenter[k1];
    p2[1] = H[0].y - massCenter[k2];
    //  float perimeter = 0.0;
    for (size_t i = 1; i < hull_noRep; i++)
    {
        p1 = p2;
        p2[0] = H[i].x - massCenter[k1];
        p2[1] = H[i].y - massCenter[k2];
        float lenght_side = (p1 - p2).norm();
        //    perimeter += lenght_side;
        covariances +=
            lenght_side * (p1 * p1.transpose() + p2 * p2.transpose());
    }
    //  covariances /= perimeter;

    // Compute eigen vectors and values
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix2f> evd(covariances);
    // Organize eigenvectors and eigenvalues in ascendent order
    for (int i = 0; i < 2; ++i)
    {
        std::cout << "Eig " << evd.eigenvalues()[i] << " v "
                  << evd.eigenvectors().col(i).transpose() << "\n";
    }
    if (evd.eigenvalues()[0] > 2 * evd.eigenvalues()[1])
    {
        elongation = sqrt(evd.eigenvalues()[0] / evd.eigenvalues()[1]);
        v3PpalDir[k1] = evd.eigenvectors()(0, 0);
        v3PpalDir[k2] = evd.eigenvectors()(1, 0);
        v3PpalDir[k0] =
            (-d - v3normal[k1] * v3PpalDir[k1] - v3normal[k2] * v3PpalDir[k2]) /
            v3normal[k0];
        v3PpalDir /= v3PpalDir.norm();
    }
}

/*!Returns true when the closest distance between the patches "this" and the
 * input "plane_nearby" is under distThreshold. This function is approximated*/
bool Plane::isPlaneNearby(Plane& plane_nearby, const float distThreshold)
{
    float distThres2 = distThreshold * distThreshold;

    // First we check distances between centroids and vertex to accelerate this
    // check
    if ((v3center - plane_nearby.v3center).squaredNorm() < distThres2)
        return true;

    for (unsigned i = 1; i < polygonContourPtr->size(); i++)
        if ((getVector3fromPointXYZ(polygonContourPtr->points[i]) -
             plane_nearby.v3center)
                .squaredNorm() < distThres2)
            return true;

    for (unsigned j = 1; j < plane_nearby.polygonContourPtr->size(); j++)
        if ((v3center -
             getVector3fromPointXYZ(plane_nearby.polygonContourPtr->points[j]))
                .squaredNorm() < distThres2)
            return true;

    for (unsigned i = 1; i < polygonContourPtr->size(); i++)
        for (unsigned j = 1; j < plane_nearby.polygonContourPtr->size(); j++)
            if ((diffPoints(
                     polygonContourPtr->points[i],
                     plane_nearby.polygonContourPtr->points[j]))
                    .squaredNorm() < distThres2)
                return true;

    // a) Between an edge and a vertex
    // b) Between two edges (imagine two polygons on perpendicular planes)
    // a) & b)
    for (unsigned i = 1; i < polygonContourPtr->size(); i++)
        for (unsigned j = 1; j < plane_nearby.polygonContourPtr->size(); j++)
            if (dist3D_Segment_to_Segment2(
                    Segment(
                        polygonContourPtr->points[i],
                        polygonContourPtr->points[i - 1]),
                    Segment(
                        plane_nearby.polygonContourPtr->points[j],
                        plane_nearby.polygonContourPtr->points[j - 1])) <
                distThres2)
                return true;

    return false;
}

/*! Returns true if the two input planes represent the same physical surface for
 * some given angle and distance thresholds.
 * If the planes are the same they are merged in this and the function returns
 * true. Otherwise it returns false.*/
bool Plane::isSamePlane(
    Plane& plane_nearby, const float& cosAngleThreshold,
    const float& parallelDistThres, const float& proxThreshold)
{
    // Check that both planes have similar orientation
    if (v3normal.dot(plane_nearby.v3normal) < cosAngleThreshold) return false;

    // Check the normal distance of the planes centers using their average
    // normal
    float dist_normal = v3normal.dot(plane_nearby.v3center - v3center);
    if (fabs(dist_normal) > parallelDistThres)  // Then merge the planes
        return false;

    // Check that the distance between the planes centers is not too big
    if (!isPlaneNearby(plane_nearby, proxThreshold)) return false;

    return true;
}

/*!Check if the the input plane is the same than this plane for some given angle
 * and distance thresholds.
 * If the planes are the same they are merged in this and the function returns
 * true. Otherwise it returns false.*/
bool Plane::isSamePlane(
    Eigen::Matrix4f& Rt, Plane& plane_, const float& cosAngleThreshold,
    const float& parallelDistThres, const float& proxThreshold)
{
    Plane plane = plane_;
    plane.v3normal = Rt.block<3, 3>(0, 0) * plane_.v3normal;
    plane.v3center =
        Rt.block<3, 3>(0, 0) * plane_.v3center + Rt.block(0, 3, 3, 1);
    pcl::transformPointCloud(
        *plane_.polygonContourPtr, *plane.polygonContourPtr, Rt);

    if (fabs(d - plane.d) > parallelDistThres) return false;

    // Check that both planes have similar orientation
    if (v3normal.dot(plane.v3normal) < cosAngleThreshold) return false;

    //  // Check the normal distance of the planes centers using their average
    //  normal
    //  float dist_normal = v3normal.dot(plane.v3center - v3center);
    ////  if(fabs(dist_normal) > parallelDistThres ) // Avoid matching different
    /// parallel planes
    ////    return false;
    //  float thres_max_dist = max(parallelDistThres,
    //  parallelDistThres*2*norm(plane.v3center - v3center));
    //  if(fabs(dist_normal) > thres_max_dist ) // Avoid matching different
    //  parallel planes
    //    return false;

    // Once we know that the planes are almost coincident (parallelism and
    // position)
    // we check that the distance between the planes is not too big
    return isPlaneNearby(plane, proxThreshold);
}

bool Plane::hasSimilarDominantColor(Plane& plane, const float colorThreshold)
{
    if (bDominantColor && plane.bDominantColor &&
        (fabs(v3colorNrgb[0] - plane.v3colorNrgb[0]) > colorThreshold ||
         fabs(v3colorNrgb[1] - plane.v3colorNrgb[1]) > colorThreshold))
        return false;
    else
        return true;
}

/*! Merge the input "same_plane_patch" into "this".
 *  Recalculate center, normal vector, area, inlier points (filtered), convex
 * hull, etc.
 */
void Plane::mergePlane(Plane& same_plane_patch)
{
    // Update normal and center
    // assert(areaVoxels > 0 && same_plane_patch.areaVoxels > 0)
    //  v3normal = (areaVoxels*v3normal +
    //  same_plane_patch.areaVoxels*same_plane_patch.v3normal);
    assert(inliers.size() > 0 && same_plane_patch.inliers.size() > 0);
    v3normal =
        (inliers.size() * v3normal +
         same_plane_patch.inliers.size() * same_plane_patch.v3normal);
    v3normal = v3normal / v3normal.norm();

    // Update point inliers
    //  *polygonContourPtr += *same_plane_patch.polygonContourPtr; // Merge
    //  polygon points
    *planePointCloudPtr +=
        *same_plane_patch.planePointCloudPtr;  // Add the points of the new
    // detection and perform a voxel
    // grid

    // Filter the points of the patch with a voxel-grid. This points are used
    // only for visualization
    static pcl::VoxelGrid<pcl::PointXYZRGBA> merge_grid;
    merge_grid.setLeafSize(0.05, 0.05, 0.05);
    pcl::PointCloud<pcl::PointXYZRGBA> mergeCloud;
    merge_grid.setInputCloud(planePointCloudPtr);
    merge_grid.filter(mergeCloud);
    planePointCloudPtr->clear();
    *planePointCloudPtr = mergeCloud;
    //  calcMainColor();
    //  if(configPbMap.use_color)
    //    calcMainColor();

    inliers.insert(
        inliers.end(), same_plane_patch.inliers.begin(),
        same_plane_patch.inliers.end());

    *same_plane_patch.polygonContourPtr += *polygonContourPtr;
    calcConvexHull(same_plane_patch.polygonContourPtr);
    computeMassCenterAndArea();

    d = -v3normal.dot(v3center);

    // Move the points to fulfill the plane equation
    forcePtsLayOnPlane();

    // Update area
    //  double area_recalc = planePointCloudPtr->size() * 0.0025;
    //  mpPlaneInferInfo->isFullExtent(same_plane_patch, area_recalc);
    areaVoxels = planePointCloudPtr->size() * 0.0025;
}

// Adaptation for RGBD360
void Plane::mergePlane2(Plane& same_plane_patch)
{
    // Update normal and center
    assert(inliers.size() > 0 && same_plane_patch.inliers.size() > 0);
    v3normal =
        (inliers.size() * v3normal +
         same_plane_patch.inliers.size() * same_plane_patch.v3normal);
    v3normal = v3normal / v3normal.norm();

    // Update point inliers
    *planePointCloudPtr +=
        *same_plane_patch.planePointCloudPtr;  // Add the points of the new
    // detection and perform a voxel
    // grid

    inliers.insert(
        inliers.end(), same_plane_patch.inliers.begin(),
        same_plane_patch.inliers.end());

    *same_plane_patch.polygonContourPtr += *polygonContourPtr;
    calcConvexHull(same_plane_patch.polygonContourPtr);
    //  calcConvexHullandParams(same_plane_patch.polygonContourPtr);
    // std::cout << d << " area " << areaHull << " center " <<
    // v3center.transpose() << " elongation " << elongation << " v3PpalDir " <<
    // v3PpalDir.transpose() << std::endl;
    computeMassCenterAndArea();

    calcElongationAndPpalDir();

    d = -v3normal.dot(v3center);
    // std::cout << d << "area " << areaHull << " center " <<
    // v3center.transpose() << " elongation " << elongation << " v3PpalDir " <<
    // v3PpalDir.transpose() << std::endl;

    calcMainColor2();  // Calculate dominant color

    // Update color histogram
    for (size_t i = 0; i < hist_H.size(); i++)
    {
        hist_H[i] += same_plane_patch.hist_H[i];
        hist_H[i] /= 2;
    }
}

void Plane::transform(Eigen::Matrix4f& Rt)
{
    // Transform normal and ppal direction
    v3normal = Rt.block<3, 3>(0, 0) * v3normal;
    v3PpalDir = Rt.block<3, 3>(0, 0) * v3PpalDir;

    // Transform centroid
    v3center = Rt.block<3, 3>(0, 0) * v3center + Rt.block(0, 3, 3, 1);

    d = -(v3normal.dot(v3center));

    // Transform convex hull points
    pcl::transformPointCloud(*polygonContourPtr, *polygonContourPtr, Rt);

    pcl::transformPointCloud(*planePointCloudPtr, *planePointCloudPtr, Rt);
}

#endif