HMT_SLAM_common.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 HMT_SLAM_common_H
#define HMT_SLAM_common_H

#include <mrpt/utils/CSerializable.h>
#include <mrpt/utils/list_searchable.h>
#include <set>

#define COMMON_TOPOLOG_HYP static_cast<THypothesisID>(0)

#define NODE_ANNOTATION_METRIC_MAPS "metricMaps"  // CMultiMetricMap
#define NODE_ANNOTATION_REF_POSEID "refPoseID"  // TPoseID
#define NODE_ANNOTATION_POSES_GRAPH "posesGraph"  // CRobotPosesGraph

#define NODE_ANNOTATION_PLACE_POSE "placePose"  // mrpt::poses::CPoint2D

#define ARC_ANNOTATION_DELTA \
        "Delta"  // CPose3DPDF (for the current implementation, it's a
// CPose3DPDFParticles)
#define ARC_ANNOTATION_DELTA_SRC_POSEID \
        "Delta_poseID_src"  // TPoseID (elemental datatype)
#define ARC_ANNOTATION_DELTA_TRG_POSEID \
        "Delta_poseID_trg"  // TPoseID (elemental datatype)

/**  Used in constructor of mrpt::hmtslam::CHMHMapArc */
#define ARC_TYPES "Membership,Navegability,RelativePose,Location"
#define DEFAULT_ARC_TYPE "Membership"

/**  Used in constructor of mrpt::hmtslam::CHMHMapNode */
#define NODE_TYPES "Place,Area,TopologicalMap,Object"
#define DEFAULT_NODE_TYPE "Place"

// Used as current robot pose when initializing an empty HMT-map.
#define POSEID_INVALID static_cast<TPoseID>(-1)

#define AREAID_INVALID static_cast<uint64_t>(-1)

#define MSG_SOURCE_LSLAM 1
#define MSG_SOURCE_AA 2

namespace mrpt
{
namespace hmtslam
{
class CHMHMapArc;
class CHMHMapNode;

/** An integer number uniquely identifying each of the concurrent hypotheses for
 * the robot topological path (& possibly local metric clusters) in HMT-SLAM.
  *   The number 0 has the special meaning of "that part of the map/robot path
 * in which all hypotheses agree".
  *  They can be generated from CHMTSLAM::generateHypothesisID()
  */
typedef int64_t THypothesisID;

/** An integer number uniquely identifying each robot pose stored in HMT-SLAM.
  * They can be generated from CHMTSLAM::generatePoseID()
  */
typedef uint64_t TPoseID;

typedef std::pair<TPoseID, TPoseID> TPairPoseIDs;

typedef std::vector<TPoseID> TPoseIDList;
typedef std::set<TPoseID> TPoseIDSet;

/** A set of hypothesis IDs, used for arcs and nodes in multi-hypothesis hybrid
 * maps.
  *  \sa THypothesisID, CHierarchicalMHMap
  * \ingroup mrpt_hmtslam_grp
  */
class THypothesisIDSet : public mrpt::utils::CSerializable,
                                                 public std::set<THypothesisID>
{
        DEFINE_SERIALIZABLE(THypothesisIDSet)

   public:
        /** Default constructor
          */
        THypothesisIDSet() {}
        /** Constructor with one initial element
          */
        THypothesisIDSet(const THypothesisID& val) { insert(val); }
        virtual ~THypothesisIDSet() {}
        /** Returns true if the hypothesis is into the set.
          */
        bool has(const THypothesisID& val) const
        {
                return find(val) != end() || find(COMMON_TOPOLOG_HYP) != end();
        }

        /** Dump to console.
          */
        void debugDump() const;
};

/** A class for storing a sequence of arcs (a path).
  * \sa CHMTSLAM
  */
class TArcList : public mrpt::utils::list_searchable<
                                         std::shared_ptr<mrpt::hmtslam::CHMHMapArc>>
{
   private:
        typedef mrpt::utils::list_searchable<
                std::shared_ptr<mrpt::hmtslam::CHMHMapArc>>
                BASE;

   public:
        void debugDump();
        void read(utils::CStream& in);
        void write(utils::CStream& out) const;
};

}  // End of namespace
}  // End of namespace

#endif