SubgraphMatcher.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 __SUBGRAPHMATCHER_H
#define __SUBGRAPHMATCHER_H

#include <mrpt/config.h>
#if MRPT_HAS_PCL

#include <mrpt/utils/utils_defs.h>

#include <mrpt/pbmap/heuristicParams.h>
#include <mrpt/pbmap/PbMap.h>
#include <mrpt/pbmap/Subgraph.h>

namespace mrpt
{
namespace pbmap
{
/*!This class finds the best correspondence between the planes of two subgraphs
 * (i.e. sets of neighbor planes).
 * It relies on an interpretation tree employing geometric restrictions that are
 * represented as a set of unary and binary constraints.
 *
 * \ingroup mrpt_pbmap_grp
 */
class SubgraphMatcher
{
   public:
        SubgraphMatcher();

        /*!Check if the two input planes fulfill a set of geometric constraints, and
         * so, if they are candidates to be the same plane.*/
        bool evalUnaryConstraints(
                Plane& plane1, Plane& plane2, PbMap& trgPbMap,
                bool useStructure = false);
        bool evalUnaryConstraints2D(
                Plane& plane1, Plane& plane2, PbMap& trgPbMap,
                bool useStructure = false);
        bool evalUnaryConstraintsOdometry(
                Plane& plane1, Plane& plane2, PbMap& trgPbMap,
                bool useStructure = false);
        bool evalUnaryConstraintsOdometry2D(
                Plane& plane1, Plane& plane2, PbMap& trgPbMap,
                bool useStructure = false);

        /*!Check if the two pair of planes plane1-plane2 ans planeA-planeB fulfill
        the same geometric relationship, and so,
        if they are candidates to be the same planes.*/
        bool evalBinaryConstraints(
                Plane& plane1, Plane& plane2, Plane& planeA, Plane& planeB);
        bool evalBinaryConstraintsOdometry(
                Plane& plane1, Plane& plane2, Plane& planeA, Plane& planeB);

        /*!List of combinations that have been explored in the interpretation tree.*/  // Cambiar nombre
        std::vector<std::map<unsigned, unsigned>> alreadyExplored;

        /*!Find the best combination of planes correspondences given two subgraphs represeting local neighborhoods of planes.*/  // Cambiar nombre o Quitar!
        void exploreSubgraphTreeR(
                std::set<unsigned>& evalRef, std::set<unsigned>& evalCheck,
                std::map<unsigned, unsigned>& matched);
        void exploreSubgraphTreeR_Area(
                std::set<unsigned>& evalRef, std::set<unsigned>& evalCheck,
                std::map<unsigned, unsigned>& matched);

        /*!Set source (current) subgraph.*/
        void inline setSourceSubgraph(Subgraph& subgSrc) { subgraphSrc = &subgSrc; }
        /*!Set target subgraph.*/
        void inline setTargetSubgraph(Subgraph& subgTrg) { subgraphTrg = &subgTrg; }
        /*!Returns a list with plane matches from subgraphSrc to subgraphTrg.*/
        //    std::map<unsigned,unsigned> compareSubgraphs(Subgraph &subgraphSource,
        //    Subgraph &subgraphTarget);
        std::map<unsigned, unsigned> compareSubgraphs(
                Subgraph& subgraphSource, Subgraph& subgraphTarget,
                const int option = 0);  // Options are

        /*!One subgraph to be matched.*/
        Subgraph* subgraphSrc;

        /*!The other subgraph to be matched.*/
        Subgraph* subgraphTrg;

        int nCheckConditions;

        int totalUnary;
        int semanticPair;
        int rejectSemantic;

        /*!Return the total area of the matched planes in the frame source.*/
        float calcAreaMatched(std::map<unsigned, unsigned>& matched_planes);

        /*!Set of thresholds for PbMap matching.*/
        config_heuristics configLocaliser;

   private:
        /*!List of planes correspondences.*/
        std::map<unsigned, unsigned> winnerMatch;
        float areaWinnerMatch;

        /*!Hash table for unary constraints.*/
        std::vector<std::vector<int8_t>> hashUnaryConstraints;

        float calcAreaUnmatched(std::set<unsigned>& unmatched_planes);
};
}
}  // End of namespaces

#endif
#endif