Plane.h

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>
 */

#pragma once

#include <mrpt/config.h>

#if MRPT_HAS_PCL

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

#define USE_COMPLETNESS_HEURISTICS 1
#define USE_INFERRED_STRUCTURE 1

static std::vector<size_t> DEFAULT_VECTOR;

namespace mrpt::pbmap
{
/** 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 Plane : public mrpt::serialization::CSerializable
{
    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 contour::Ptr;
    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;
};
}  // namespace mrpt::pbmap
#endif