CHMTSLAM.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, 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 |
   +------------------------------------------------------------------------+ */
#ifndef CHMTSLAM_H
#define CHMTSLAM_H
 
#include <mrpt/system/COutputLogger.h>
#include <mrpt/containers/CThreadSafeQueue.h>
 
#include <mrpt/hmtslam/HMT_SLAM_common.h>
#include <mrpt/hmtslam/CLocalMetricHypothesis.h>
#include <mrpt/hmtslam/CHierarchicalMHMap.h>
#include <mrpt/hmtslam/CTopLCDetector_GridMatching.h>
#include <mrpt/hmtslam/CTopLCDetector_FabMap.h>
#include <mrpt/slam/CICP.h>
#include <mrpt/maps/CPointsMap.h>
#include <mrpt/slam/TKLDParams.h>
#include <mrpt/obs/CActionCollection.h>
#include <mrpt/opengl/COpenGLScene.h>
#include <mrpt/serialization/CMessage.h>
#include <mrpt/core/aligned_std_map.h>
 
#include <thread>
#include <queue>
 
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: http://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() : log_lik(0), delta_new_cur(0) {}
                        /** Log likelihood for this loop-closure. */
                        double log_lik;
 
                        /** 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;
                };
 
                /** The meat is here: only feasible loop closures from "cur_area" are
                 * included here, with associated data.
                  */
                std::map<CHMHMapNode::TNodeID, TBI_info> loopClosureData;
 
                // MRPT_MAKE_ALIGNED_OPERATOR_NEW
        };
 
        /** 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
          */
        virtual ~CHMTSLAM();
 
        /** 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. */
        mrpt::aligned_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() {}
        /** 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
          */
        virtual ~CLSLAM_RBPF_2DLASER();
 
        /** 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);
 
        /** 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);
 
        /** 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);
 
   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
 
}  // End of namespace
}  // End of namespace
 
#endif