Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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 
56  };
58 
59 
60  /** This class is used in HMT-SLAM to represent each of the Local Metric Hypotheses (LMHs).
61  * It has a set of particles representing the robot path in nearby poses.
62  * \sa CHMTSLAM, CLSLAM_RBPF_2DLASER
63  */
65  public mrpt::bayes::CParticleFilterData<CLSLAMParticleData>,
66  public mrpt::bayes::CParticleFilterDataImpl<CLocalMetricHypothesis,mrpt::bayes::CParticleFilterData<CLSLAMParticleData>::CParticleList>,
67  public mrpt::utils::CSerializable
68  {
70 
71  // This must be added to any CSerializable derived class:
73 
74  public:
75  /** Constructor (Default param only used from STL classes)
76  */
77  CLocalMetricHypothesis( CHMTSLAM * parent = NULL );
78 
79  /** Destructor
80  */
82 
83  synch::CCriticalSection m_lock; //!< Critical section for threads signaling they are working with the LMH.
84  THypothesisID m_ID; //!< The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
85  mrpt::utils::safe_ptr<CHMTSLAM> m_parent; //!< For quick access to our parent object.
86  TPoseID m_currentRobotPose; //!< The current robot pose (its global unique ID) for this hypothesis.
87  //TNodeIDList m_neighbors; //!< The list of all areas sourronding the current one (this includes the current area itself).
88  TNodeIDSet m_neighbors; //!< The list of all areas sourronding the current one (this includes the current area itself).
89  std::map<TPoseID,CHMHMapNode::TNodeID> m_nodeIDmemberships; //!< The hybrid map node membership for each robot pose.
90  std::map<TPoseID,mrpt::obs::CSensoryFrame> m_SFs; //!< The SF gathered at each robot pose.
91  TPoseIDList m_posesPendingAddPartitioner; //!< The list of poseIDs waiting to be added to the graph partitioner, what happens in the LSLAM thread main loop.
92  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
93 
94  double m_log_w; //!< Log-weight of this hypothesis.
95  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.
96  //std::map<TPoseID,double> m_log_w_topol_history; //!< The historic log-weights of the topological observations inserted in this LMH.
97 
98  mrpt::obs::CActionRobotMovement2D m_accumRobotMovement; //!< Used in CLSLAM_RBPF_2DLASER
99  bool m_accumRobotMovementIsValid; //!< Used in CLSLAM_RBPF_2DLASER
100 
101  /** Used by AA thread */
103  {
104  synch::CCriticalSection lock; //!< CS to access the entire struct.
106  std::map<uint32_t,TPoseID> idx2pose; //!< For the poses in "partitioner".
107 
108  unsigned int pose2idx(const TPoseID &id) const; //!< Uses idx2pose to perform inverse searches.
109 
110  } m_robotPosesGraph;
111 
112  /** 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.
113  * The metric maps are *not* included here for convenience, call m_metricMaps.getAs3DScene().
114  * The previous contents of "objs" will be discarded
115  */
116  void getAs3DScene( mrpt::opengl::CSetOfObjectsPtr &objs ) const;
117 
118  /** Returns the mean of each robot pose in this LMH, as computed from the set of particles.
119  * \sa getPathParticles, getRelativePose
120  */
121  void getMeans( TMapPoseID2Pose3D &outList ) const;
122 
123  /** Returns the mean and covariance of each robot pose in this LMH, as computed from the set of particles.
124  * \sa getMeans, getPoseParticles
125  */
126  void getPathParticles( std::map< TPoseID, mrpt::poses::CPose3DPDFParticles > &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, getPathParticles
130  */
131  void getPoseParticles( const TPoseID &poseID, mrpt::poses::CPose3DPDFParticles &outPDF ) const;
132 
133  /** Returns the pose PDF of some pose relative to some other pose ID (both must be part of the the LMH).
134  * \sa getMeans, getPoseParticles
135  */
136  void getRelativePose(
137  const TPoseID &reference,
138  const TPoseID &pose,
139  mrpt::poses::CPose3DPDFParticles &outPDF ) const;
140 
141  /** Describes the LMH in text.
142  */
143  void dumpAsText(utils::CStringList &st) const;
144 
145  /** 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.
146  */
147  void changeCoordinateOrigin( const TPoseID &newOrigin );
148 
149  /** Rebuild the metric maps of all particles from the observations and their estimated poses. */
150  void rebuildMetricMaps();
151 
152  /** Rebuild the auxiliary metric maps in "m_robotPosesGraph" from the observations "m_SFs" and their estimated poses. */
153  //void rebuildSSOMatrix();
154 
155  /** Sets the number of particles to the initial number according to the PF options, and initialize them with no robot poses & empty metric maps.
156  */
157  void clearRobotPoses();
158 
159  /** Returns the i'th particle hypothesis for the current robot pose. */
160  const mrpt::poses::CPose3D * getCurrentPose(const size_t &particleIdx) const;
161 
162  /** Returns the i'th particle hypothesis for the current robot pose. */
163  mrpt::poses::CPose3D * getCurrentPose(const size_t &particleIdx);
164 
165  /** Removes a given area from the LMH:
166  * - The corresponding node in the HMT map is updated with the robot poses & SFs in the LMH.
167  * - Robot poses belonging to that area are removed from:
168  * - the particles.
169  * - the graph partitioner.
170  * - the list of SFs.
171  * - the list m_nodeIDmemberships.
172  * - m_neighbors is updated.
173  * - The weights of all particles are changed to remove the effects of the removed metric observations.
174  * - After calling this the metric maps should be updated.
175  * - This method internally calls updateAreaFromLMH
176  */
177  void removeAreaFromLMH( const CHMHMapNode::TNodeID areaID );
178 
179  /** 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.
180  * If eraseSFsFromLMH=true, the sensoryframes are moved rather than copied to the area, and removed from the LMH.
181  * \note The critical section m_map_cs is locked internally, unlock it before calling this.
182  */
183  void updateAreaFromLMH(
184  const CHMHMapNode::TNodeID areaID,
185  bool eraseSFsFromLMH = false );
186 
187 
188  protected:
189 
190  /** @name Virtual methods for Particle Filter implementation (just a wrapper interface, actually implemented in CHMTSLAM::m_LSLAM_method)
191  @{
192  */
193 
194  /** The PF algorithm implementation.
195  */
196  void prediction_and_update_pfAuxiliaryPFOptimal(
197  const mrpt::obs::CActionCollection * action,
198  const mrpt::obs::CSensoryFrame * observation,
200 
201  /** The PF algorithm implementation. */
202  void prediction_and_update_pfOptimalProposal(
203  const mrpt::obs::CActionCollection * action,
204  const mrpt::obs::CSensoryFrame * observation,
206  /** @}
207  */
208 
209 
210  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
211  */
212  mutable mrpt::math::CVectorDouble m_pfAuxiliaryPFOptimal_estimatedProb;
213 
214  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
215  */
216  mutable std::vector<double> m_maxLikelihood;
217 
218  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. */
219  mutable mrpt::poses::StdVector_CPose2D m_movementDraws;
220 
221  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. */
222  mutable unsigned int m_movementDrawsIdx;
223 
224  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. */
225  mutable mrpt::poses::StdVector_CPose2D m_movementDrawMaximumLikelihood;
226 
227  }; // End of class def.
229 
230  } // End of namespace
231 } // End of namespace
232 
233 #endif
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
THypothesisID m_ID
The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:39
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
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...
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
double m_log_w
Log-weight of this hypothesis.
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
Definition: CHMTSLAM.h:59
Declares a class for storing a collection of robot actions.
mrpt::obs::CActionRobotMovement2D m_accumRobotMovement
Used in CLSLAM_RBPF_2DLASER.
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
Represents a probabilistic 2D movement of the robot mobile base.
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...
TPoseIDList m_posesPendingAddPartitioner
The list of poseIDs waiting to be added to the graph partitioner, what happens in the LSLAM thread ma...
std::map< TPoseID, CHMHMapNode::TNodeID > m_nodeIDmemberships
The hybrid map node membership for each robot pose.
mrpt::utils::safe_ptr< CHMTSLAM > m_parent
For quick access to our parent object.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
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...
bool m_accumRobotMovementIsValid
Used in CLSLAM_RBPF_2DLASER.
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.
std::map< TPoseID, mrpt::obs::CSensoryFrame > m_SFs
The SF gathered at each robot pose.
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.
synch::CCriticalSection m_lock
Critical section for threads signaling they are working with the LMH.
std::set< CHMHMapNode::TNodeID > TNodeIDSet
Definition: CHMHMapNode.h:150
This class stores any customizable set of metric maps.
CLSLAMParticleData(const mrpt::maps::TSetOfMetricMapInitializers *mapsInitializers=NULL)
synch::CCriticalSection lock
CS to access the entire struct.
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
TNodeIDList m_areasPendingTBI
The list of area IDs waiting to be processed by the TBI (topological bayesian inference) engines to s...
TNodeIDSet m_neighbors
The list of all areas sourronding the current one (this includes the current area itself)...
mrpt::aligned_containers< CPose2D >::vector_t StdVector_CPose2D
Eigen aligment-compatible container.
Definition: CPose2D.h:272
A wrapper class for pointers that can be safely copied with "=" operator without problems.
Definition: safe_pointers.h:64
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
TPoseID m_currentRobotPose
The current robot pose (its global unique ID) for this hypothesis.
mrpt::maps::CMultiMetricMap metricMaps



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019