Main MRPT website > C++ reference for MRPT 1.4.0
CLocalMetricHypothesis.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 #ifndef CLocalMetricHypothesis_H
10 #define CLocalMetricHypothesis_H
11 
14 
17 
22 
23 #include <list>
24 
25 namespace mrpt
26 {
27  namespace poses { class CPose3DPDFParticles; }
28 
29  namespace hmtslam
30  {
32 
35 
38 
39  /** 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).
40  * \ingroup mrpt_hmtslam_grp
41  */
42  class HMTSLAM_IMPEXP CLSLAMParticleData : public mrpt::utils::CSerializable
43  {
44  // This must be added to any CSerializable derived class:
46 
47  public:
48  CLSLAMParticleData( const mrpt::maps::TSetOfMetricMapInitializers *mapsInitializers = NULL ) :
49  metricMaps( mapsInitializers ),
50  robotPoses()
51  {
52  }
53 
55  {
56  robotPoses.clear();
57  }
58 
60  TMapPoseID2Pose3D robotPoses;
61  };
63 
64 
65  /** This class is used in HMT-SLAM to represent each of the Local Metric Hypotheses (LMHs).
66  * It has a set of particles representing the robot path in nearby poses.
67  * \sa CHMTSLAM, CLSLAM_RBPF_2DLASER
68  */
70  public mrpt::bayes::CParticleFilterData<CLSLAMParticleData>,
71  public mrpt::bayes::CParticleFilterDataImpl<CLocalMetricHypothesis,mrpt::bayes::CParticleFilterData<CLSLAMParticleData>::CParticleList>,
72  public mrpt::utils::CSerializable
73  {
75 
76  // This must be added to any CSerializable derived class:
77  DEFINE_SERIALIZABLE( CLocalMetricHypothesis )
78 
79  public:
80  /** Constructor (Default param only used from STL classes)
81  */
82  CLocalMetricHypothesis( CHMTSLAM * parent = NULL );
83 
84  /** Destructor
85  */
86  ~CLocalMetricHypothesis();
87 
88  synch::CCriticalSection m_lock; //!< Critical section for threads signaling they are working with the LMH.
89  THypothesisID m_ID; //!< The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
90  mrpt::utils::safe_ptr<CHMTSLAM> m_parent; //!< For quick access to our parent object.
91  TPoseID m_currentRobotPose; //!< The current robot pose (its global unique ID) for this hypothesis.
92  //TNodeIDList m_neighbors; //!< The list of all areas sourronding the current one (this includes the current area itself).
93  TNodeIDSet m_neighbors; //!< The list of all areas sourronding the current one (this includes the current area itself).
94  std::map<TPoseID,CHMHMapNode::TNodeID> m_nodeIDmemberships; //!< The hybrid map node membership for each robot pose.
95  std::map<TPoseID,mrpt::obs::CSensoryFrame> m_SFs; //!< The SF gathered at each robot pose.
96  TPoseIDList m_posesPendingAddPartitioner; //!< The list of poseIDs waiting to be added to the graph partitioner, what happens in the LSLAM thread main loop.
97  TNodeIDList m_areasPendingTBI; //!< 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
98 
99  double m_log_w; //!< Log-weight of this hypothesis.
100  std::vector<std::map<TPoseID,double> > m_log_w_metric_history; //!< The historic log-weights of the metric observations inserted in this LMH, for each particle.
101  //std::map<TPoseID,double> m_log_w_topol_history; //!< The historic log-weights of the topological observations inserted in this LMH.
102 
103  mrpt::obs::CActionRobotMovement2D m_accumRobotMovement; //!< Used in CLSLAM_RBPF_2DLASER
104  bool m_accumRobotMovementIsValid; //!< Used in CLSLAM_RBPF_2DLASER
105 
106  /** Used by AA thread */
108  {
109  synch::CCriticalSection lock; //!< CS to access the entire struct.
111  std::map<uint32_t,TPoseID> idx2pose; //!< For the poses in "partitioner".
112 
113  unsigned int pose2idx(const TPoseID &id) const; //!< Uses idx2pose to perform inverse searches.
114 
115  } m_robotPosesGraph;
116 
117  /** 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.
118  * The metric maps are *not* included here for convenience, call m_metricMaps.getAs3DScene().
119  * The previous contents of "objs" will be discarded
120  */
121  void getAs3DScene( mrpt::opengl::CSetOfObjectsPtr &objs ) const;
122 
123  /** Returns the mean of each robot pose in this LMH, as computed from the set of particles.
124  * \sa getPathParticles, getRelativePose
125  */
126  void getMeans( TMapPoseID2Pose3D &outList ) const;
127 
128  /** Returns the mean and covariance of each robot pose in this LMH, as computed from the set of particles.
129  * \sa getMeans, getPoseParticles
130  */
131  void getPathParticles( std::map< TPoseID, mrpt::poses::CPose3DPDFParticles > &outList ) const;
132 
133  /** Returns the mean and covariance of each robot pose in this LMH, as computed from the set of particles.
134  * \sa getMeans, getPathParticles
135  */
136  void getPoseParticles( const TPoseID &poseID, mrpt::poses::CPose3DPDFParticles &outPDF ) const;
137 
138  /** Returns the pose PDF of some pose relative to some other pose ID (both must be part of the the LMH).
139  * \sa getMeans, getPoseParticles
140  */
141  void getRelativePose(
142  const TPoseID &reference,
143  const TPoseID &pose,
144  mrpt::poses::CPose3DPDFParticles &outPDF ) const;
145 
146  /** Describes the LMH in text.
147  */
148  void dumpAsText(utils::CStringList &st) const;
149 
150  /** 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.
151  */
152  void changeCoordinateOrigin( const TPoseID &newOrigin );
153 
154  /** Rebuild the metric maps of all particles from the observations and their estimated poses. */
155  void rebuildMetricMaps();
156 
157  /** Rebuild the auxiliary metric maps in "m_robotPosesGraph" from the observations "m_SFs" and their estimated poses. */
158  //void rebuildSSOMatrix();
159 
160  /** Sets the number of particles to the initial number according to the PF options, and initialize them with no robot poses & empty metric maps.
161  */
162  void clearRobotPoses();
163 
164  /** Returns the i'th particle hypothesis for the current robot pose. */
165  const mrpt::poses::CPose3D * getCurrentPose(const size_t &particleIdx) const;
166 
167  /** Returns the i'th particle hypothesis for the current robot pose. */
168  mrpt::poses::CPose3D * getCurrentPose(const size_t &particleIdx);
169 
170  /** Removes a given area from the LMH:
171  * - The corresponding node in the HMT map is updated with the robot poses & SFs in the LMH.
172  * - Robot poses belonging to that area are removed from:
173  * - the particles.
174  * - the graph partitioner.
175  * - the list of SFs.
176  * - the list m_nodeIDmemberships.
177  * - m_neighbors is updated.
178  * - The weights of all particles are changed to remove the effects of the removed metric observations.
179  * - After calling this the metric maps should be updated.
180  * - This method internally calls updateAreaFromLMH
181  */
182  void removeAreaFromLMH( const CHMHMapNode::TNodeID areaID );
183 
184  /** 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.
185  * If eraseSFsFromLMH=true, the sensoryframes are moved rather than copied to the area, and removed from the LMH.
186  * \note The critical section m_map_cs is locked internally, unlock it before calling this.
187  */
188  void updateAreaFromLMH(
189  const CHMHMapNode::TNodeID areaID,
190  bool eraseSFsFromLMH = false );
191 
192 
193  protected:
194 
195  /** @name Virtual methods for Particle Filter implementation (just a wrapper interface, actually implemented in CHMTSLAM::m_LSLAM_method)
196  @{
197  */
198 
199  /** The PF algorithm implementation.
200  */
201  void prediction_and_update_pfAuxiliaryPFOptimal(
202  const mrpt::obs::CActionCollection * action,
203  const mrpt::obs::CSensoryFrame * observation,
205 
206  /** The PF algorithm implementation. */
207  void prediction_and_update_pfOptimalProposal(
208  const mrpt::obs::CActionCollection * action,
209  const mrpt::obs::CSensoryFrame * observation,
211  /** @}
212  */
213 
214 
215  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
216  */
217  mutable mrpt::math::CVectorDouble m_pfAuxiliaryPFOptimal_estimatedProb;
218 
219  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
220  */
221  mutable std::vector<double> m_maxLikelihood;
222 
223  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. */
224  mutable mrpt::poses::StdVector_CPose2D m_movementDraws;
225 
226  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. */
227  mutable unsigned int m_movementDrawsIdx;
228 
229  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. */
230  mutable mrpt::poses::StdVector_CPose2D m_movementDrawMaximumLikelihood;
231 
232  }; // End of class def.
234 
235  } // End of namespace
236 } // End of namespace
237 
238 #endif
#define HMTSLAM_IMPEXP
This class provides simple critical sections functionality.
Implements a 2D local SLAM method based on a RBPF over an occupancy grid map.
Definition: CHMTSLAM.h:512
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:39
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
std::map< TYPE1, TYPE2, std::less< TYPE1 >, Eigen::aligned_allocator< std::pair< const TYPE1, TYPE2 > > > map_t
STL namespace.
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
Definition: CHMTSLAM.h:59
Declares a class for storing a collection of robot actions.
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
A class for storing a list of text lines.
Definition: CStringList.h:32
#define DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
uint64_t TNodeID
The type for node IDs in graphs of different types.
Definition: types_simple.h:45
This class is used in HMT-SLAM to represent each of the Local Metric Hypotheses (LMHs).
std::map< uint32_t, TPoseID > idx2pose
For the poses in "partitioner".
std::vector< TPoseID > TPoseIDList
Auxiliary class used in mrpt::slam::CLocalMetricHypothesis for HMT-SLAM; this class keeps the data re...
mrpt::aligned_containers< TPoseID, mrpt::poses::CPose3D >::map_t TMapPoseID2Pose3D
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
This class can be used to make partitions on a map/graph build from observations taken at some poses/...
std::deque< CParticleData > CParticleList
Use this type to refer to the list of particles m_particles.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
The configuration of a particle filter.
std::set< CHMHMapNode::TNodeID > TNodeIDSet
Definition: CHMHMapNode.h:150
This class stores any customizable set of metric maps.
synch::CCriticalSection lock
CS to access the entire struct.
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
A class for representing a node in a hierarchical, multi-hypothesis map.
Definition: CHMHMapNode.h:37
mrpt::aligned_containers< CPose2D >::vector_t StdVector_CPose2D
Eigen aligment-compatible container.
Definition: CPose2D.h:265
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
mrpt::utils::TNodeID TNodeID
The type of the IDs of nodes.
Definition: CHMHMapNode.h:49
mrpt::maps::CMultiMetricMap metricMaps



Page generated by Doxygen 1.8.11 for MRPT 1.4.0 SVN:Unversioned directory at Tue Jun 28 11:46:25 UTC 2016