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/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
{
/** 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::utils::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;
};
}
}  // End of namespaces

#endif

#endif