CHMTSLAM.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          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/containers/CThreadSafeQueue.h>
#include <mrpt/system/COutputLogger.h>

#include <mrpt/hmtslam/CHierarchicalMHMap.h>
#include <mrpt/hmtslam/CLocalMetricHypothesis.h>
#include <mrpt/hmtslam/CTopLCDetector_FabMap.h>
#include <mrpt/hmtslam/CTopLCDetector_GridMatching.h>
#include <mrpt/hmtslam/HMT_SLAM_common.h>
#include <mrpt/maps/CPointsMap.h>
#include <mrpt/obs/CActionCollection.h>
#include <mrpt/opengl/COpenGLScene.h>
#include <mrpt/serialization/CMessage.h>
#include <mrpt/slam/CICP.h>
#include <mrpt/slam/TKLDParams.h>
#include <map>

#include <queue>
#include <thread>

namespace mrpt
{
/** Classes related to the implementation of Hybrid Metric Topological (HMT)
 * SLAM. \ingroup mrpt_hmtslam_grp */
namespace hmtslam
{
class CHMTSLAM;
class CLSLAMAlgorithmBase;
class CLSLAM_RBPF_2DLASER;

/** An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
 *
 *   The main entry points for a user are pushAction() and pushObservations().
 *Several parameters can be modified
 *   through m_options.
 *
 *  The mathematical models of this approach have been reported in:
 *      - Blanco J.L., Fernandez-Madrigal J.A., and Gonzalez J., "Towards a
 *Unified Bayesian Approach to Hybrid Metric-Topological SLAM", in  IEEE
 *Transactions on Robotics (TRO), Vol. 24, No. 2, April 2008.
 *      - ...
 *
 *  More information in the wiki page: https://www.mrpt.org/HMT-SLAM . A
 *complete working application can be found in "MRPT/apps/hmt-slam".
 *
 *  The complete state of the SLAM framework is serializable, so it can be saved
 *and restore to/from a binary dump. This class implements
 *mrpt::serialization::CSerializable, so
 *    it can be saved with "stream << slam_object;" and restored with "stream >>
 *slam_object;". Alternatively, the methods CHMTSLAM::saveState and
 *CHMTSLAM::loadState
 *    can be invoked, which in turn call internally to the CSerializable
 *interface.
 *
 * \sa CHierarchicalMHMap
 * \ingroup mrpt_hmtslam_grp
 */
class CHMTSLAM : public mrpt::system::COutputLogger,
                 public mrpt::serialization::CSerializable
{
    friend class CLocalMetricHypothesis;
    friend class CLSLAM_RBPF_2DLASER;
    friend class CTopLCDetector_GridMatching;
    friend class CTopLCDetector_FabMap;

    DEFINE_SERIALIZABLE(CHMTSLAM)

   protected:
    /** @name Inter-thread communication queues:
        @{ */
    /** Message definition:
        - From: LSLAM
        - To: AA
        - Meaning: Reconsider the graph partition for the given LMH. Compute SSO
       for the new node(s) "newPoseIDs".
        */
    /*struct TMessageAA
    {
        CLocalMetricHypothesis  *LMH;
        TPoseIDList             newPoseIDs;
    };*/

    /** Message definition:
        - From: AA
        - To: LSLAM
        - Meaning: The partitioning of robot poses.
        */
    struct TMessageLSLAMfromAA
    {
        using Ptr = std::shared_ptr<TMessageLSLAMfromAA>;
        /** The hypothesis ID (LMH) for these results. */
        THypothesisID hypothesisID;
        std::vector<TPoseIDList> partitions;

        /** for debugging only */
        void dumpToConsole() const;
    };

    /** Message definition:
        - From: LSLAM
        - To: TBI
        - Meaning: One or more areas are to be considered by the TBI engines.
        */
    struct TMessageLSLAMtoTBI
    {
        using Ptr = std::shared_ptr<TMessageLSLAMtoTBI>;
        /** The LMH */
        CLocalMetricHypothesis* LMH;
        /** The areas to consider. */
        TNodeIDList areaIDs;
    };

    /** Message definition:
        - From: TBI
        - To: LSLAM
        - Meaning:
        */
    struct TMessageLSLAMfromTBI
    {
        using Ptr = std::shared_ptr<TMessageLSLAMfromTBI>;
        /** The hypothesis ID (LMH) for these results. */
        THypothesisID hypothesisID;

        /** The area for who the loop-closure has been computed. */
        CHMHMapNode::TNodeID cur_area;

        struct TBI_info
        {
            TBI_info() = default;
            /** Log likelihood for this loop-closure. */
            double log_lik{0};

            /** Depending on the loop-closure engine, an guess of the relative
             * pose of "area_new" relative to "cur_area" is given here.
             *  If the SOG contains 0 modes, then the engine does not provide
             * this information.
             */
            mrpt::poses::CPose3DPDFSOG delta_new_cur{0};
        };

        /** The meat is here: only feasible loop closures from "cur_area" are
         * included here, with associated data.
         */
        std::map<CHMHMapNode::TNodeID, TBI_info> loopClosureData;

        //
    };

    /** LSLAM thread input queue, messages of type CHMTSLAM::TMessageLSLAMfromAA
     */
    using CMessageQueue =
        mrpt::containers::CThreadSafeQueue<mrpt::serialization::CMessage>;

    CMessageQueue m_LSLAM_queue;

    /** @} */

    /** The Area Abstraction (AA) method, invoked from LSLAM.
     * \param LMH (IN) The LMH which to this query applies.
     * \param newPoseIDs (IN) The new poseIDs to be added to the graph
     * partitioner.
     * \return A structure with all return data. Memory to be freed by user.
     * \note The critical section for LMH must be locked BEFORE calling this
     * method (it does NOT lock any critical section).
     */
    static TMessageLSLAMfromAA::Ptr areaAbstraction(
        CLocalMetricHypothesis* LMH, const TPoseIDList& newPoseIDs);

    /** The entry point for Topological Bayesian Inference (TBI) engines,
     * invoked from LSLAM.
     * \param LMH (IN) The LMH which to this query applies.
     * \param areaID (IN) The area ID to consider for potential loop-closures.
     * \note The critical section for LMH must be locked BEFORE calling this
     * method (it does NOT lock any critical section).
     */
    static TMessageLSLAMfromTBI::Ptr TBI_main_method(
        CLocalMetricHypothesis* LMH, const CHMHMapNode::TNodeID& areaID);

    /** @name Related to the input queue:
        @{ */
   public:
    /** Empty the input queue. */
    void clearInputQueue();

    /** Returns true if the input queue is empty (Note that the queue must not
     * be empty to the user to enqueue more actions/observaitions)
     * \sa pushAction,pushObservations, inputQueueSize
     */
    bool isInputQueueEmpty();

    /** Returns the number of objects waiting for processing in the input queue.
     * \sa pushAction,pushObservations, isInputQueueEmpty
     */
    size_t inputQueueSize();

    /** Here the user can enter an action into the system (will go to the SLAM
     * process).
     *  This class will delete the passed object when required, so DO NOT
     * DELETE the passed object after calling this.
     * \sa pushObservations,pushObservation
     */
    void pushAction(const mrpt::obs::CActionCollection::Ptr& acts);

    /** Here the user can enter observations into the system (will go to the
     * SLAM process).
     *  This class will delete the passed object when required, so DO NOT
     * DELETE the passed object after calling this.
     * \sa pushAction,pushObservation
     */
    void pushObservations(const mrpt::obs::CSensoryFrame::Ptr& sf);

    /** Here the user can enter an observation into the system (will go to the
     * SLAM process).
     *  This class will delete the passed object when required, so DO NOT
     * DELETE the passed object after calling this.
     * \sa pushAction,pushObservation
     */
    void pushObservation(const mrpt::obs::CObservation::Ptr& obs);

    enum TLSlamMethod
    {
        lsmRBPF_2DLASER = 1
    };

   protected:
    /** Used from the LSLAM thread to retrieve the next object from the queue.
     * \return The object, or nullptr if empty.
     */
    mrpt::serialization::CSerializable::Ptr getNextObjectFromInputQueue();

    /** The queue of pending actions/observations supplied by the user waiting
     * for being processed. */
    std::queue<mrpt::serialization::CSerializable::Ptr> m_inputQueue;

    /** Critical section for accessing  m_inputQueue */
    mutable std::mutex m_inputQueue_cs;

    /** Critical section for accessing m_map */
    mutable std::mutex m_map_cs;

    /** Critical section for accessing m_LMHs */
    mutable std::mutex m_LMHs_cs;

    /** @} */

    /** @name Threads stuff
        @{ */

    /** The function for the "Local SLAM" thread. */
    void thread_LSLAM();

    /** The function for the "TBI" thread. */
    void thread_TBI();

    /** The function for the "3D viewer" thread. */
    void thread_3D_viewer();
    /** Threads handles */
    std::thread m_hThread_LSLAM, m_hThread_TBI, m_hThread_3D_viewer;
    /** @} */

    /** @name HMT-SLAM sub-processes.
        @{ */
    /** Auxiliary method within thread_LSLAM */
    void LSLAM_process_message(const mrpt::serialization::CMessage& msg);

    /** No critical section locks are assumed at the entrance of this method.
     */
    void LSLAM_process_message_from_AA(const TMessageLSLAMfromAA& myMsg);

    /** No critical section locks are assumed at the entrance of this method.
     */
    void LSLAM_process_message_from_TBI(const TMessageLSLAMfromTBI& myMsg);

    /** Topological Loop Closure: Performs all the required operations
        to close a loop between two areas which have been determined
        to be the same.
     */
    void perform_TLC(
        CLocalMetricHypothesis& LMH, const CHMHMapNode::TNodeID areaInLMH,
        const CHMHMapNode::TNodeID areaLoopClosure,
        const mrpt::poses::CPose3DPDFGaussian& pose1wrt2);

    /** @} */

    /** @name The different SLAM algorithms that can be invoked from the LSLAM
       thread.
        @{ */

    /** An instance of a local SLAM method, to be applied to each LMH -
     * initialized by "initializeEmptyMap" or "loadState".
     */
    CLSLAMAlgorithmBase* m_LSLAM_method;

    /** @} */

    /** @name The different Loop-Closure modules that are to be executed in the
       TBI thread.
        @{ */
   protected:
    using TLopLCDetectorFactory = CTopLCDetectorBase* (*)(CHMTSLAM*);

    std::map<std::string, TLopLCDetectorFactory> m_registeredLCDetectors;

    /** The list of LC modules in operation - initialized by
     * "initializeEmptyMap" or "loadState".  */
    std::deque<CTopLCDetectorBase*> m_topLCdets;

    /** The critical section for accessing m_topLCdets */
    std::mutex m_topLCdets_cs;

   public:
    /** Must be invoked before calling  initializeEmptyMap, so LC objects can be
     * created. */
    void registerLoopClosureDetector(
        const std::string& name,
        CTopLCDetectorBase* (*ptrCreateObject)(CHMTSLAM*));

    /** The class factory for topological loop closure detectors.
     *  Possible values are enumerated in TOptions::TLC_detectors
     *
     * \exception std::exception On unknown name.
     */
    CTopLCDetectorBase* loopClosureDetector_factory(const std::string& name);

    /** @} */
   protected:
    /** Termination flag for signaling all threads to terminate */
    bool m_terminateThreads;

    /** Threads termination flags:
     */
    bool m_terminationFlag_LSLAM, m_terminationFlag_TBI,
        m_terminationFlag_3D_viewer;

    /** Generates a new and unique area textual label (currently this generates
     * "0","1",...) */
    static std::string generateUniqueAreaLabel();

    /** Generates a new and unique pose ID */
    static TPoseID generatePoseID();

    /** Generates a new and unique hypothesis ID */
    static THypothesisID generateHypothesisID();

    static int64_t m_nextAreaLabel;
    static TPoseID m_nextPoseID;
    static THypothesisID m_nextHypID;

   public:
    /** Default constructor
     *  \param debug_out_stream If debug output messages should be redirected
     * to any other stream apart from std::cout
     */
    CHMTSLAM();

    CHMTSLAM(const CHMTSLAM&) : mrpt::system::COutputLogger()
    {
        THROW_EXCEPTION("This object cannot be copied.");
    }
    const CHMTSLAM& operator=(const CHMTSLAM&)
    {
        THROW_EXCEPTION("This object cannot be copied.");
    }

    /** Destructor
     */
    ~CHMTSLAM() override;

    /** Return true if an exception has been caught in any thread leading to the
     * end of the mapping application: no more actions/observations will be
     * processed from now on.
     */
    bool abortedDueToErrors();

    /** @name High-level map management
        @{ */

    /** Loads the options from a config file. */
    void loadOptions(const std::string& configFile);
    /** Loads the options from a config source */
    void loadOptions(const mrpt::config::CConfigFileBase& cfgSource);

    /** Initializes the whole HMT-SLAM framework, reseting to an empty map (It
     * also clears the logs directory) - this must be called AFTER loading the
     * options with CHMTSLAM::loadOptions. */
    void initializeEmptyMap();

    /** Save the state of the whole HMT-SLAM framework to some binary stream
     * (e.g. a file).
     * \return true if everything goes OK.
     * \sa loadState
     */
    bool saveState(mrpt::serialization::CArchive& out) const;

    /** Load the state of the whole HMT-SLAM framework from some binary stream
     * (e.g. a file).
     * \return true if everything goes OK.
     * \sa saveState
     */
    bool loadState(mrpt::serialization::CArchive& in);
    /** @} */

    /** @name The important data.
        @{ */
    /** The hiearchical, multi-hypothesis graph-based map. */
    CHierarchicalMHMap m_map;
    /** The list of LMHs at each instant. */
    std::map<THypothesisID, CLocalMetricHypothesis> m_LMHs;
    /** @} */

    /** Called from LSLAM thread when log files must be created.
     */
    void generateLogFiles(unsigned int nIteration);

    /** Gets a 3D representation of the current state of the whole mapping
     * framework.
     */
    void getAs3DScene(mrpt::opengl::COpenGLScene& outScene);

   protected:
    /** A variety of options and configuration params (private, use
     * loadOptions).
     */
    struct TOptions : public mrpt::config::CLoadableOptions
    {
        /** Initialization of default params
         */
        TOptions();

        void loadFromConfigFile(
            const mrpt::config::CConfigFileBase& source,
            const std::string& section) override;  // See base docs
        void dumpToTextStream(
            std::ostream& out) const override;  // See base docs

        /** [LOGGING] If it is not an empty string (""), a directory with that
         * name will be created and log files save there. */
        std::string LOG_OUTPUT_DIR;
        /** [LOGGING] One SLAM iteration out of "LOGGING_logFrequency", a log
         * file will be generated. */
        int LOG_FREQUENCY;

        /** [LSLAM] The method to use for local SLAM
         */
        TLSlamMethod SLAM_METHOD;

        /** [LSLAM] Minimum distance (and heading) difference between
         * observations inserted in the map.
         */
        float SLAM_MIN_DIST_BETWEEN_OBS, SLAM_MIN_HEADING_BETWEEN_OBS;

        /** [LSLAM] Minimum uncertainty (1 sigma, meters) in x and y for
         * odometry increments (Default=0) */
        float MIN_ODOMETRY_STD_XY;

        /** [LSLAM] Minimum uncertainty (1 sigma, rads) in phi for odometry
         * increments (Default=0) */
        float MIN_ODOMETRY_STD_PHI;

        /** [VIEW3D] The height of the areas' spheres.
         */
        float VIEW3D_AREA_SPHERES_HEIGHT;

        /** [VIEW3D] The radius of the areas' spheres.
         */
        float VIEW3D_AREA_SPHERES_RADIUS;

        /** A 3-length vector with the std. deviation of the transition model in
         * (x,y,phi) used only when there is no odometry (if there is odo, its
         * uncertainty values will be used instead); x y: In meters, phi:
         * radians (but in degrees when loading from a configuration ini-file!)
         */
        mrpt::math::CVectorFloat stds_Q_no_odo;

        /** [AA] The options for the partitioning algorithm
         */
        mrpt::slam::CIncrementalMapPartitioner::TOptions AA_options;

        /** The default set of maps to be created in each particle */
        mrpt::maps::TSetOfMetricMapInitializers defaultMapsInitializers;
        /** These params are used from every LMH object. */
        bayes::CParticleFilter::TParticleFilterOptions pf_options;
        mrpt::slam::TKLDParams KLD_params;

        /** 0 means randomize, use any other value to have repetitive
         * experiments. */
        int random_seed;

        /** A list of topological loop-closure detectors to use: can be one or
         * more from this list:
         *  'gridmaps': Occupancy Grid matching.
         *  'fabmap': Mark Cummins' image matching framework.
         */
        std::vector<std::string> TLC_detectors;

        /** Options passed to this TLC constructor */
        CTopLCDetector_GridMatching::TOptions TLC_grid_options;
        /** Options passed to this TLC constructor */
        CTopLCDetector_FabMap::TOptions TLC_fabmap_options;

    } m_options;

};  // End of class CHMTSLAM.

/** Virtual base for local SLAM methods, used in mrpt::slam::CHMTSLAM.
 */
class CLSLAMAlgorithmBase
{
    friend class CLocalMetricHypothesis;

   protected:
    mrpt::safe_ptr<CHMTSLAM> m_parent;

   public:
    /** Constructor
     */
    CLSLAMAlgorithmBase(CHMTSLAM* parent) : m_parent(parent) {}
    /** Destructor
     */
    virtual ~CLSLAMAlgorithmBase() = default;
    /** Main entry point from HMT-SLAM: process some actions & observations.
     *  The passed action/observation will be deleted, so a copy must be made
     * if necessary.
     *  This method must be in charge of updating the robot pose estimates and
     * also to update the
     *   map when required.
     *
     * \param LMH   The local metric hypothesis which must be updated by this
     * SLAM algorithm.
     * \param act   The action to process (or nullptr).
     * \param sf    The observations to process (or nullptr).
     */
    virtual void processOneLMH(
        CLocalMetricHypothesis* LMH,
        const mrpt::obs::CActionCollection::Ptr& act,
        const mrpt::obs::CSensoryFrame::Ptr& sf) = 0;

    /** The PF algorithm implementation.
     */
    virtual void prediction_and_update_pfAuxiliaryPFOptimal(
        CLocalMetricHypothesis* LMH, const mrpt::obs::CActionCollection* action,
        const mrpt::obs::CSensoryFrame* observation,
        const bayes::CParticleFilter::TParticleFilterOptions& PF_options) = 0;

    /** The PF algorithm implementation.  */
    virtual void prediction_and_update_pfOptimalProposal(
        CLocalMetricHypothesis* LMH, const mrpt::obs::CActionCollection* action,
        const mrpt::obs::CSensoryFrame* observation,
        const bayes::CParticleFilter::TParticleFilterOptions& PF_options) = 0;

};  // end of class CLSLAMAlgorithmBase

/** Implements a 2D local SLAM method based on a RBPF over an occupancy grid
 * map.
 *  This class is used internally in mrpt::slam::CHMTSLAM
 */
class CLSLAM_RBPF_2DLASER : public CLSLAMAlgorithmBase
{
    friend class CLocalMetricHypothesis;

   public:
    /** Constructor
     */
    CLSLAM_RBPF_2DLASER(CHMTSLAM* parent);

    /** Destructor
     */
    ~CLSLAM_RBPF_2DLASER() override;

    /** Main entry point from HMT-SLAM: process some actions & observations.
     *  The passed action/observation will be deleted, so a copy must be made
     * if necessary.
     *  This method must be in charge of updating the robot pose estimates and
     * also to update the
     *   map when required.
     *
     * \param LMH   The local metric hypothesis which must be updated by this
     * SLAM algorithm.
     * \param act   The action to process (or nullptr).
     * \param sf    The observations to process (or nullptr).
     */
    void processOneLMH(
        CLocalMetricHypothesis* LMH,
        const mrpt::obs::CActionCollection::Ptr& act,
        const mrpt::obs::CSensoryFrame::Ptr& sf) override;

    /** The PF algorithm implementation.  */
    void prediction_and_update_pfAuxiliaryPFOptimal(
        CLocalMetricHypothesis* LMH, const mrpt::obs::CActionCollection* action,
        const mrpt::obs::CSensoryFrame* observation,
        const bayes::CParticleFilter::TParticleFilterOptions& PF_options)
        override;

    /** The PF algorithm implementation.  */
    void prediction_and_update_pfOptimalProposal(
        CLocalMetricHypothesis* LMH, const mrpt::obs::CActionCollection* action,
        const mrpt::obs::CSensoryFrame* observation,
        const bayes::CParticleFilter::TParticleFilterOptions& PF_options)
        override;

   protected:
    /**  For use within PF callback methods */
    bool m_insertNewRobotPose;

    /** Auxiliary structure
     */
    struct TPathBin
    {
        TPathBin() : x(), y(), phi() {}
        std::vector<int> x, y, phi;

        /** For debugging purposes!
         */
        void dumpToStdOut() const;
    };

    /** Fills out a "TPathBin" variable, given a path hypotesis and (if not set
     * to nullptr) a new pose appended at the end, using the KLD params in
     * "options".
     */
    void loadTPathBinFromPath(
        TPathBin& outBin, TMapPoseID2Pose3D* path = nullptr,
        mrpt::poses::CPose2D* newPose = nullptr);

    /** Checks if a given "TPathBin" element is already into a set of them, and
     * return its index (first one is 0), or -1 if not found.
     */
    int findTPathBinIntoSet(TPathBin& desiredBin, std::deque<TPathBin>& theSet);

    /** Auxiliary function used in "prediction_and_update_pfAuxiliaryPFOptimal"
     */
    static double particlesEvaluator_AuxPFOptimal(
        const mrpt::bayes::CParticleFilter::TParticleFilterOptions& PF_options,
        const mrpt::bayes::CParticleFilterCapable* obj, size_t index,
        const void* action, const void* observation);

    /** Auxiliary function that evaluates the likelihood of an observation,
     * given a robot pose, and according to the options in
     * "CPosePDFParticles::options".
     */
    static double auxiliarComputeObservationLikelihood(
        const mrpt::bayes::CParticleFilter::TParticleFilterOptions& PF_options,
        const mrpt::bayes::CParticleFilterCapable* obj,
        size_t particleIndexForMap, const mrpt::obs::CSensoryFrame* observation,
        const mrpt::poses::CPose2D* x);

};  // end class CLSLAM_RBPF_2DLASER

}  // namespace hmtslam
}  // namespace mrpt