CLocalMetricHypothesis.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 |
   +------------------------------------------------------------------------+ */
#ifndef CLocalMetricHypothesis_H
#define CLocalMetricHypothesis_H

#include <mrpt/hmtslam/HMT_SLAM_common.h>
#include <mrpt/hmtslam/CHMHMapNode.h>

#include <mrpt/maps/CMultiMetricMap.h>
#include <mrpt/obs/CActionRobotMovement2D.h>
#include <mrpt/slam/CIncrementalMapPartitioner.h>
#include <mrpt/opengl/opengl_frwds.h>

#include <list>

namespace mrpt
{
namespace bayes
{
class CParticleFilter;
}
namespace poses
{
class CPose3DPDFParticles;
}

namespace hmtslam
{
typedef mrpt::aligned_containers<TPoseID, mrpt::poses::CPose3D>::map_t
        TMapPoseID2Pose3D;

class CHMTSLAM;
class CLSLAM_RBPF_2DLASER;

/** Auxiliary class used in mrpt::slam::CLocalMetricHypothesis for HMT-SLAM;
 * this class keeps the data relative to each local metric particle ("a robot
 * metric path hypothesis" and its associated metric map).
 * \ingroup mrpt_hmtslam_grp
 */
class CLSLAMParticleData : public mrpt::utils::CSerializable
{
        DEFINE_SERIALIZABLE(CLSLAMParticleData)

   public:
        CLSLAMParticleData(
                const mrpt::maps::TSetOfMetricMapInitializers* mapsInitializers =
                        nullptr)
                : metricMaps(mapsInitializers), robotPoses()
        {
        }

        mrpt::maps::CMultiMetricMap metricMaps;
        TMapPoseID2Pose3D robotPoses;
};

class CLSLAMParticleDataParticles
        : public mrpt::bayes::CParticleFilterData<CLSLAMParticleData>,
          public mrpt::bayes::CParticleFilterDataImpl<
                  mrpt::bayes::CParticleFilterData<CLSLAMParticleData>,
                  mrpt::bayes::CParticleFilterData<CLSLAMParticleData>::CParticleList>
{
};

/** This class is used in HMT-SLAM to represent each of the Local Metric
 * Hypotheses (LMHs).
 *   It has a set of particles representing the robot path in nearby poses.
 * \sa CHMTSLAM, CLSLAM_RBPF_2DLASER
 */
class CLocalMetricHypothesis : public mrpt::utils::CSerializable
{
        friend class CLSLAM_RBPF_2DLASER;

        DEFINE_SERIALIZABLE(CLocalMetricHypothesis)

   public:
        using CParticleData = CLSLAMParticleDataParticles::CParticleData;
        using CParticleList =
                mrpt::bayes::CParticleFilterData<CLSLAMParticleData>::CParticleList;
        CLSLAMParticleDataParticles m_poseParticles;
        /** Constructor (Default param only used from STL classes)
         */
        CLocalMetricHypothesis(CHMTSLAM* parent = nullptr);

        /** Destructor
         */
        ~CLocalMetricHypothesis();

        MRPT_TODO(
                "Separate the serializable class from this code, so we don't have to "
                "worry about copying locks")
        struct ThreadLocks
        {
                // Don't really copy mutexes
                ThreadLocks() {}
                ThreadLocks(const ThreadLocks&) {}
                mutable std::mutex m_lock;
        } threadLocks;
        /** The unique ID of the hypothesis (Used for accessing
         * mrpt::slam::CHierarchicalMHMap). */
        THypothesisID m_ID;
        /** For quick access to our parent object. */
        mrpt::utils::safe_ptr<CHMTSLAM> m_parent;
        /** The current robot pose (its global unique ID) for this hypothesis. */
        TPoseID m_currentRobotPose;
        // TNodeIDList                                                  m_neighbors;                    //!< The
        // list of all areas sourronding
        // the current one (this includes the current area itself).
        /** The list of all areas sourronding the current one (this includes the
         * current area itself). */
        TNodeIDSet m_neighbors;
        /** The hybrid map node membership for each robot pose. */
        std::map<TPoseID, CHMHMapNode::TNodeID> m_nodeIDmemberships;
        /** The SF gathered at each robot pose. */
        std::map<TPoseID, mrpt::obs::CSensoryFrame> m_SFs;
        /** The list of poseIDs waiting to be added to the graph partitioner, what
         * happens in the LSLAM thread main loop. */
        TPoseIDList m_posesPendingAddPartitioner;
        /** The list of area IDs waiting to be processed by the TBI (topological
         * bayesian inference) engines to search for potential loop-closures. Set in
         * CHMTSLAM::LSLAM_process_message_from_AA, read in */
        TNodeIDList m_areasPendingTBI;

        /** Log-weight of this hypothesis. */
        double m_log_w;
        /** The historic log-weights of the metric observations inserted in this
         * LMH, for each particle. */
        std::vector<std::map<TPoseID, double>> m_log_w_metric_history;
        // std::map<TPoseID,double>     m_log_w_topol_history;          //!< The historic
        // log-weights of the topological observations inserted in this LMH.

        /** Used in CLSLAM_RBPF_2DLASER */
        mrpt::obs::CActionRobotMovement2D m_accumRobotMovement;
        /** Used in CLSLAM_RBPF_2DLASER */
        bool m_accumRobotMovementIsValid;

        /** Used by AA thread */
        struct TRobotPosesPartitioning
        {
                TRobotPosesPartitioning() {}
                TRobotPosesPartitioning(const TRobotPosesPartitioning& o)
                        : partitioner(o.partitioner), idx2pose(o.idx2pose)
                {
                }
                /** CS to access the entire struct. */
                mutable std::mutex lock;
                mrpt::slam::CIncrementalMapPartitioner partitioner;
                /** For the poses in "partitioner". */
                std::map<uint32_t, TPoseID> idx2pose;

                /** Uses idx2pose to perform inverse searches. */
                unsigned int pose2idx(const TPoseID& id) const;

        } m_robotPosesGraph;

        /** Returns a 3D representation of the the current robot pose, all the poses
         * in the auxiliary graph, and each of the areas they belong to.
         *  The metric maps are *not* included here for convenience, call
         * m_metricMaps.getAs3DScene().
         *  The previous contents of "objs" will be discarded
         */
        void getAs3DScene(mrpt::opengl::CSetOfObjects::Ptr& objs) const;

        /** Returns the mean of each robot pose in this LMH, as computed from the
         * set of particles.
         * \sa getPathParticles, getRelativePose
         */
        void getMeans(TMapPoseID2Pose3D& outList) const;

        /** Returns the mean and covariance of each robot pose in this LMH, as
         * computed from the set of particles.
         * \sa getMeans, getPoseParticles
         */
        void getPathParticles(
                std::map<TPoseID, mrpt::poses::CPose3DPDFParticles>& outList) const;

        /** Returns the mean and covariance of each robot pose in this LMH, as
         * computed from the set of particles.
         * \sa getMeans, getPathParticles
         */
        void getPoseParticles(
                const TPoseID& poseID, mrpt::poses::CPose3DPDFParticles& outPDF) const;

        /** Returns the pose PDF of some pose relative to some other pose ID (both
         * must be part of the the LMH).
         * \sa getMeans, getPoseParticles
         */
        void getRelativePose(
                const TPoseID& reference, const TPoseID& pose,
                mrpt::poses::CPose3DPDFParticles& outPDF) const;

        /** Describes the LMH in text.
         */
        void dumpAsText(utils::CStringList& st) const;

        /** Change all coordinates to set a given robot pose as the new coordinate
         * origin, and rebuild metric maps and change coords in the partitioning
         * subsystem as well.
         */
        void changeCoordinateOrigin(const TPoseID& newOrigin);

        /** Rebuild the metric maps of all particles from the observations and their
         * estimated poses. */
        void rebuildMetricMaps();

        /** Rebuild the auxiliary metric maps in "m_robotPosesGraph" from the
         * observations "m_SFs" and their estimated poses. */
        // void rebuildSSOMatrix();

        /** Sets the number of particles to the initial number according to the PF
         * options, and initialize them with no robot poses & empty metric maps.
         */
        void clearRobotPoses();

        /** Returns the i'th particle hypothesis for the current robot pose.  */
        const mrpt::poses::CPose3D* getCurrentPose(const size_t& particleIdx) const;

        /** Returns the i'th particle hypothesis for the current robot pose.  */
        mrpt::poses::CPose3D* getCurrentPose(const size_t& particleIdx);

        /** Removes a given area from the LMH:
         *      - The corresponding node in the HMT map is updated with the robot poses
         *& SFs in the LMH.
         *      - Robot poses belonging to that area are removed from:
         *              - the particles.
         *              - the graph partitioner.
         *              - the list of SFs.
         *              - the list m_nodeIDmemberships.
         *      - m_neighbors is updated.
         * - The weights of all particles are changed to remove the effects of the
         *removed metric observations.
         *      - After calling this the metric maps should be updated.
         * - This method internally calls updateAreaFromLMH
         */
        void removeAreaFromLMH(const CHMHMapNode::TNodeID areaID);

        /** The corresponding node in the HMT map is updated with the robot poses &
         * SFs in the LMH: the poses are referenced to the area's reference poseID,
         * such as that reference is at the origin.
         *  If eraseSFsFromLMH=true, the sensoryframes are moved rather than copied
         * to the area, and removed from the LMH.
         * \note The critical section m_map_cs is locked internally, unlock it
         * before calling this.
         */
        void updateAreaFromLMH(
                const CHMHMapNode::TNodeID areaID, bool eraseSFsFromLMH = false);

        /** @name Virtual methods for Particle Filter implementation (just a wrapper
           interface, actually implemented in CHMTSLAM::m_LSLAM_method)
                @{
         */

        /** The PF algorithm implementation.
         */
        template <class PF_ALGORITHM>
        void prediction_and_update(
                const mrpt::obs::CActionCollection* action,
                const mrpt::obs::CSensoryFrame* observation,
                const bayes::CParticleFilter::TParticleFilterOptions& PF_options);

        /** Auxiliary function used in "prediction_and_update_pfAuxiliaryPFOptimal"
         */
        double particlesEvaluator_AuxPFOptimal(
                const mrpt::bayes::CParticleFilter::TParticleFilterOptions& PF_options,
                size_t index, const mrpt::obs::CSensoryFrame* observation);

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

        void executeOn(
                        mrpt::bayes::CParticleFilter& pf, const mrpt::obs::CActionCollection* action,
                        const mrpt::obs::CSensoryFrame* observation,
                        mrpt::bayes::CParticleFilter::TParticleFilterStats* stats,
                        mrpt::bayes::CParticleFilter::TParticleFilterAlgorithm PF_algorithm);


   protected:
        /** @}
         */

        /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
         */
        mrpt::math::CVectorDouble m_pfAuxiliaryPFOptimal_estimatedProb;

        /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
         */
        std::vector<double> m_maxLikelihood;

        /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. */
        unsigned int m_movementDrawsIdx;

        /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. */
        mrpt::poses::StdVector_CPose2D m_movementDraws;

        /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. */
        mrpt::poses::StdVector_CPose2D m_movementDrawMaximumLikelihood;

};  // End of class def.

}  // namespace hmtslam
}  // namespace mrpt

#endif