Plane.h

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://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>
 */

#ifndef __PBMAP_PLANE_H
#define __PBMAP_PLANE_H

#include <mrpt/config.h>

#if MRPT_HAS_PCL

#include <mrpt/utils/utils_defs.h>
#include <mrpt/pbmap/link_pragmas.h>

#include <mrpt/utils/CSerializable.h>
#include <pcl/point_types.h>
#include <pcl/common/pca.h>
#include <set>
#include <map>

#define USE_COMPLETNESS_HEURISTICS 1
#define USE_INFERRED_STRUCTURE 1

static std::vector<size_t> DEFAULT_VECTOR;

namespace mrpt {
namespace pbmap {
        // This must be added to any CSerializable derived class:
        DEFINE_SERIALIZABLE_PRE_CUSTOM_LINKAGE( Plane, PBMAP_IMPEXP)

        /** A class used to store a planar feature (Plane for short).
         *  It is described with geometric features representing the shape and relative
         *  location of the patch (area, normal vector, elongation, 3D-convex hull, etc.)
         *  and radiometric features (the most representative color).
         *
         * \ingroup mrpt_pbmap_grp
         */
  class PBMAP_IMPEXP Plane : public mrpt::utils::CSerializable
  {
    // This must be added to any CSerializable derived class:
        DEFINE_SERIALIZABLE( Plane )

   public:
    Plane() :
      elongation(1.0),
      bFullExtent(false),
      bFromStructure(false),
//      contourPtr(new pcl::PointCloud<pcl::PointXYZRGBA>),
      polygonContourPtr(new pcl::PointCloud<pcl::PointXYZRGBA>),
      planePointCloudPtr(new pcl::PointCloud<pcl::PointXYZRGBA>)
    {
//      vector< vector<int> > vec(4, vector<int>(4));
    }

    /*!
     * Force the plane inliers to lay on the plane
     */
    void forcePtsLayOnPlane();

    /**!
     * Calculate the plane's convex hull with the monotone chain algorithm.
    */
//    void calcConvexHull(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &pointCloud );
    void calcConvexHull(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &pointCloud, std::vector<size_t> &indices = DEFAULT_VECTOR );

    void calcConvexHullandParams(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &pointCloud, std::vector<size_t> &indices = DEFAULT_VECTOR );

    /** \brief Compute the area of a 2D planar polygon patch - using a given normal
//      * \param polygonContourPtr the point cloud (planar)
//      * \param normal the plane normal
      */
    float compute2DPolygonalArea (/*pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &polygonContourPtr, Vector<3> &normal*/);

    /** \brief Compute the patch's convex-hull area and mass center
      */
    void computeMassCenterAndArea();

    /*!
     * Calculate plane's elongation and principal direction
     */
    void calcElongationAndPpalDir();


    /*!Returns true when the closest distance between the patches "this" and "plane" is under distThreshold.*/
    bool isPlaneNearby(Plane &plane, const float distThreshold);

    /*! 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 isSamePlane(Plane &plane, const float &cosAngleThreshold, const float &distThreshold, const float &proxThreshold);

    bool isSamePlane(Eigen::Matrix4f &Rt, Plane &plane_, const float &cosAngleThreshold, const float &distThreshold, const float &proxThreshold);

    bool hasSimilarDominantColor(Plane &plane, const float colorThreshold);

    /*! Merge the two input patches into "updatePlane".
     *  Recalculate center, normal vector, area, inlier points (filtered), convex hull, etc.
     */
    void mergePlane(Plane &plane);
    void mergePlane2(Plane &plane);// Adaptation for RGBD360

    void transform(Eigen::Matrix4f &Rt);


    /**!
     *  Parameters to allow the plane-based representation of the map by a graph
    */
    unsigned id;
    unsigned numObservations;
    unsigned semanticGroup;
    std::set<unsigned> nearbyPlanes;
    std::map<unsigned,unsigned> neighborPlanes;

    /*!Labels to store semantic attributes*/
    std::string label;
    std::string label_object;
    std::string label_context;

    /**!
     *  Geometric description
    */
    Eigen::Vector3f v3center;
    Eigen::Vector3f v3normal;
    float d;
    Eigen::Matrix4f information; // Fisher information matrix (the inverse of the plane covariance)
    float curvature;
    Eigen::Vector3f v3PpalDir;
    float elongation; // This is the reatio between the lengths of the plane in the two principal directions
    float areaVoxels;
    float areaHull;
    bool bFullExtent;
    bool bFromStructure;
    unsigned nFramesAreaIsStable;

    /**!
     *  Radiometric description
    */
    Eigen::Vector3f v3colorNrgb;
    float dominantIntensity;
    bool bDominantColor;
    Eigen::Vector3f v3colorNrgbDev;

    Eigen::Vector3f v3colorC1C2C3; // Color paper
    std::vector<float> hist_H; // Normalized, Saturated Hue histogram (including 2 bins for black and white)

    std::vector<double> prog_area;
    std::vector<double> prog_elongation; // This is the reatio between the lengths of the plane in the two principal directions
    std::vector<Eigen::Vector3f> prog_C1C2C3;
    std::vector<Eigen::Vector3f> prog_Nrgb;
    std::vector<float> prog_intensity;
    std::vector<std::vector<float> > prog_hist_H;

    /**!
     *  Convex Hull
    */
//    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr contourPtr;
    std::vector<int32_t> inliers;
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr polygonContourPtr;
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr outerPolygonPtr; // This is going to be deprecated
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr planePointCloudPtr; // This is going to be deprecated

    /*!
     * Calculate plane's main color using "MeanShift" method
     */
    void calcMainColor();
    void calcMainColor2();
    void calcPlaneHistH();

   private:
    /*!
     * Calculate plane's main color in normalized rgb space
     */
    void getPlaneNrgb();
    std::vector<float> r;
    std::vector<float> g;
    std::vector<float> b;
    std::vector<float> intensity;

    // Color paper
    /*!
     * Calculate plane's main color in C1C2C3 representation
     */
    std::vector<float> c1;
    std::vector<float> c2;
    std::vector<float> c3;
    void getPlaneC1C2C3();

    /*!
     * Calculate plane's main color in HSV representation
     */
//    vector<float> H;
//    vector<float> S;
//    vector<vector<float> > HSV;

  };
        DEFINE_SERIALIZABLE_POST_CUSTOM_LINKAGE( Plane, PBMAP_IMPEXP)

} } // End of namespaces

#endif

#endif