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 
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:
78 
79  public:
80  /** Constructor (Default param only used from STL classes)
81  */
82  CLocalMetricHypothesis( CHMTSLAM * parent = NULL );
83 
84  /** Destructor
85  */
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  */
142  const TPoseID &reference,
143  const TPoseID &pose,
144  mrpt::poses::CPose3DPDFParticles &outPDF ) const;
145 
146  /** Describes the LMH in text.
147  */
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. */
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  */
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  */
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  */
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  */
202  const mrpt::obs::CActionCollection * action,
203  const mrpt::obs::CSensoryFrame * observation,
205 
206  /** The PF algorithm implementation. */
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  */
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. */
225 
226  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. */
227  mutable unsigned int m_movementDrawsIdx;
228 
229  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. */
231 
232  }; // End of class def.
234 
235  } // End of namespace
236 } // End of namespace
237 
238 #endif
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
#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...
std::deque< CParticleData > CParticleList
Use this type to refer to the list of particles m_particles.
mrpt::utils::TNodeID TNodeID
The type of the IDs of nodes.
Definition: CHMHMapNode.h:49
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
Definition: CHMTSLAM.h:60
Implements a 2D local SLAM method based on a RBPF over an occupancy grid map.
Definition: CHMTSLAM.h:513
Auxiliary class used in mrpt::slam::CLocalMetricHypothesis for HMT-SLAM; this class keeps the data re...
CLSLAMParticleData(const mrpt::maps::TSetOfMetricMapInitializers *mapsInitializers=NULL)
mrpt::maps::CMultiMetricMap metricMaps
This class is used in HMT-SLAM to represent each of the Local Metric Hypotheses (LMHs).
mrpt::poses::StdVector_CPose2D m_movementDrawMaximumLikelihood
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
std::map< TPoseID, mrpt::obs::CSensoryFrame > m_SFs
The SF gathered at each robot pose.
TNodeIDList m_areasPendingTBI
The list of area IDs waiting to be processed by the TBI (topological bayesian inference) engines to s...
void rebuildMetricMaps()
Rebuild the metric maps of all particles from the observations and their estimated poses.
void dumpAsText(utils::CStringList &st) const
Describes the LMH in text.
CLocalMetricHypothesis(CHMTSLAM *parent=NULL)
Constructor (Default param only used from STL classes)
void getRelativePose(const TPoseID &reference, const TPoseID &pose, 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).
void getAs3DScene(mrpt::opengl::CSetOfObjectsPtr &objs) const
Returns a 3D representation of the the current robot pose, all the poses in the auxiliary graph,...
double m_log_w
Log-weight of this hypothesis.
synch::CCriticalSection m_lock
Critical section for threads signaling they are working with the LMH.
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...
THypothesisID m_ID
The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
mrpt::utils::safe_ptr< CHMTSLAM > m_parent
For quick access to our parent object.
void clearRobotPoses()
Rebuild the auxiliary metric maps in "m_robotPosesGraph" from the observations "m_SFs" and their esti...
mrpt::poses::CPose3D * getCurrentPose(const size_t &particleIdx)
Returns the i'th particle hypothesis for the current robot pose.
TNodeIDSet m_neighbors
The list of all areas sourronding the current one (this includes the current area itself).
void removeAreaFromLMH(const CHMHMapNode::TNodeID areaID)
Removes a given area from the LMH:
mrpt::obs::CActionRobotMovement2D m_accumRobotMovement
Used in CLSLAM_RBPF_2DLASER.
bool m_accumRobotMovementIsValid
Used in CLSLAM_RBPF_2DLASER.
std::vector< double > m_maxLikelihood
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
const mrpt::poses::CPose3D * getCurrentPose(const size_t &particleIdx) const
Returns the i'th particle hypothesis for the current robot pose.
void prediction_and_update_pfAuxiliaryPFOptimal(const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
The PF algorithm implementation.
mrpt::math::CVectorDouble m_pfAuxiliaryPFOptimal_estimatedProb
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
void updateAreaFromLMH(const CHMHMapNode::TNodeID areaID, bool eraseSFsFromLMH=false)
The corresponding node in the HMT map is updated with the robot poses & SFs in the LMH: the poses are...
mrpt::poses::StdVector_CPose2D m_movementDraws
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
void changeCoordinateOrigin(const TPoseID &newOrigin)
Change all coordinates to set a given robot pose as the new coordinate origin, and rebuild metric map...
std::map< TPoseID, CHMHMapNode::TNodeID > m_nodeIDmemberships
The hybrid map node membership for each robot pose.
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.
unsigned int m_movementDrawsIdx
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
TPoseID m_currentRobotPose
The current robot pose (its global unique ID) for this hypothesis.
void getPoseParticles(const TPoseID &poseID, mrpt::poses::CPose3DPDFParticles &outPDF) const
Returns the mean and covariance of each robot pose in this LMH, as computed from the set of particles...
void getMeans(TMapPoseID2Pose3D &outList) const
Returns the mean of each robot pose in this LMH, as computed from the set of particles.
void prediction_and_update_pfOptimalProposal(const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
The PF algorithm implementation.
TPoseIDList m_posesPendingAddPartitioner
The list of poseIDs waiting to be added to the graph partitioner, what happens in the LSLAM thread ma...
This class stores any customizable set of metric maps.
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
Definition: types_math.h:65
Declares a class for storing a collection of robot actions.
Represents a probabilistic 2D movement of the robot mobile base.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:73
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
This class can be used to make partitions on a map/graph build from observations taken at some poses/...
This class provides simple critical sections functionality.
The virtual base class which provides a unified interface for all persistent objects in MRPT.
Definition: CSerializable.h:40
A class for storing a list of text lines.
Definition: CStringList.h:33
#define HMTSLAM_IMPEXP
mrpt::aligned_containers< TPoseID, mrpt::poses::CPose3D >::map_t TMapPoseID2Pose3D
std::set< CHMHMapNode::TNodeID > TNodeIDSet
Definition: CHMHMapNode.h:150
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
std::vector< TPoseID > TPoseIDList
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
mrpt::aligned_containers< CPose2D >::vector_t StdVector_CPose2D
Eigen aligment-compatible container.
Definition: CPose2D.h:265
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
std::map< TYPE1, TYPE2, std::less< TYPE1 >, Eigen::aligned_allocator< std::pair< const TYPE1, TYPE2 > > > map_t
The configuration of a particle filter.
synch::CCriticalSection lock
CS to access the entire struct.
unsigned int pose2idx(const TPoseID &id) const
Uses idx2pose to perform inverse searches.
std::map< uint32_t, TPoseID > idx2pose
For the poses in "partitioner".
A wrapper class for pointers that can be safely copied with "=" operator without problems.
Definition: safe_pointers.h:65



Page generated by Doxygen 1.9.1 for MRPT 1.4.0 SVN: at Mon Apr 18 03:37:47 UTC 2022