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  */
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  */
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
mrpt::hmtslam::TPoseIDList
std::vector< TPoseID > TPoseIDList
Definition: HMT_SLAM_common.h:69
mrpt::hmtslam::CLocalMetricHypothesis::TRobotPosesPartitioning
Used by AA thread.
Definition: CLocalMetricHypothesis.h:107
mrpt::hmtslam::CLocalMetricHypothesis
This class is used in HMT-SLAM to represent each of the Local Metric Hypotheses (LMHs).
Definition: CLocalMetricHypothesis.h:69
mrpt::hmtslam::CLSLAM_RBPF_2DLASER
Implements a 2D local SLAM method based on a RBPF over an occupancy grid map.
Definition: CHMTSLAM.h:512
mrpt::hmtslam::CLocalMetricHypothesis::m_ID
THypothesisID m_ID
The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
Definition: CLocalMetricHypothesis.h:89
mrpt::hmtslam::CLSLAMParticleData::robotPoses
TMapPoseID2Pose3D robotPoses
Definition: CLocalMetricHypothesis.h:60
mrpt::bayes::CParticleFilterData< CLSLAMParticleData >::CParticleList
std::deque< CParticleData > CParticleList
Use this type to refer to the list of particles m_particles.
Definition: CParticleFilterData.h:182
mrpt::hmtslam::CLocalMetricHypothesis::m_accumRobotMovementIsValid
bool m_accumRobotMovementIsValid
Used in CLSLAM_RBPF_2DLASER.
Definition: CLocalMetricHypothesis.h:104
opengl_frwds.h
mrpt::hmtslam::CLSLAMParticleData::metricMaps
mrpt::maps::CMultiMetricMap metricMaps
Definition: CLocalMetricHypothesis.h:59
CHMHMapNode.h
mrpt::math::dynamic_vector
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
Definition: eigen_frwds.h:35
mrpt::hmtslam::CHMTSLAM
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
Definition: CHMTSLAM.h:59
mrpt::hmtslam::CLocalMetricHypothesis::m_lock
synch::CCriticalSection m_lock
Critical section for threads signaling they are working with the LMH.
Definition: CLocalMetricHypothesis.h:88
mrpt::hmtslam::CLocalMetricHypothesis::m_movementDraws
mrpt::poses::StdVector_CPose2D m_movementDraws
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
Definition: CLocalMetricHypothesis.h:224
HMT_SLAM_common.h
mrpt::hmtslam::CLocalMetricHypothesis::TRobotPosesPartitioning::partitioner
mrpt::slam::CIncrementalMapPartitioner partitioner
Definition: CLocalMetricHypothesis.h:110
mrpt::hmtslam::CLocalMetricHypothesis::m_accumRobotMovement
mrpt::obs::CActionRobotMovement2D m_accumRobotMovement
Used in CLSLAM_RBPF_2DLASER.
Definition: CLocalMetricHypothesis.h:103
mrpt::hmtslam::THypothesisID
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
Definition: HMT_SLAM_common.h:60
mrpt::hmtslam::CLSLAMParticleData
Auxiliary class used in mrpt::slam::CLocalMetricHypothesis for HMT-SLAM; this class keeps the data re...
Definition: CLocalMetricHypothesis.h:42
mrpt::obs::CActionCollection
Declares a class for storing a collection of robot actions.
Definition: obs/CActionCollection.h:30
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
mrpt::obs::CActionRobotMovement2D
Represents a probabilistic 2D movement of the robot mobile base.
Definition: obs/CActionRobotMovement2D.h:29
mrpt::hmtslam::CLSLAMParticleData::CLSLAMParticleData
CLSLAMParticleData(const mrpt::maps::TSetOfMetricMapInitializers *mapsInitializers=NULL)
Definition: CLocalMetricHypothesis.h:48
mrpt::hmtslam::CLocalMetricHypothesis::m_posesPendingAddPartitioner
TPoseIDList m_posesPendingAddPartitioner
The list of poseIDs waiting to be added to the graph partitioner, what happens in the LSLAM thread ma...
Definition: CLocalMetricHypothesis.h:96
mrpt::maps::TSetOfMetricMapInitializers
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
Definition: TMetricMapInitializer.h:71
mrpt::hmtslam::CLocalMetricHypothesis::m_currentRobotPose
TPoseID m_currentRobotPose
The current robot pose (its global unique ID) for this hypothesis.
Definition: CLocalMetricHypothesis.h:91
mrpt::hmtslam::CLocalMetricHypothesis::m_movementDrawsIdx
unsigned int m_movementDrawsIdx
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
Definition: CLocalMetricHypothesis.h:227
mrpt::obs::CSensoryFrame
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: obs/CSensoryFrame.h:52
mrpt::aligned_containers::map_t
std::map< TYPE1, TYPE2, std::less< TYPE1 >, Eigen::aligned_allocator< std::pair< const TYPE1, TYPE2 > > > map_t
Definition: aligned_containers.h:31
mrpt::hmtslam::TNodeIDSet
std::set< CHMHMapNode::TNodeID > TNodeIDSet
Definition: CHMHMapNode.h:150
mrpt::hmtslam::CLocalMetricHypothesis::TRobotPosesPartitioning::idx2pose
std::map< uint32_t, TPoseID > idx2pose
For the poses in "partitioner".
Definition: CLocalMetricHypothesis.h:111
mrpt::hmtslam::CLocalMetricHypothesis::m_movementDrawMaximumLikelihood
mrpt::poses::StdVector_CPose2D m_movementDrawMaximumLikelihood
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
Definition: CLocalMetricHypothesis.h:230
mrpt::synch::CCriticalSection
This class provides simple critical sections functionality.
Definition: CCriticalSection.h:31
CMultiMetricMap.h
mrpt::maps::CMultiMetricMap
This class stores any customizable set of metric maps.
Definition: maps/CMultiMetricMap.h:122
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
Definition: CSerializable.h:174
mrpt::hmtslam::CLocalMetricHypothesis::m_neighbors
TNodeIDSet m_neighbors
The list of all areas sourronding the current one (this includes the current area itself).
Definition: CLocalMetricHypothesis.h:93
mrpt::hmtslam::TPoseID
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
Definition: HMT_SLAM_common.h:65
mrpt::poses::CPose3DPDFParticles
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
Definition: CPose3DPDFParticles.h:33
mrpt::hmtslam::CLocalMetricHypothesis::m_log_w_metric_history
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.
Definition: CLocalMetricHypothesis.h:100
mrpt::hmtslam::CLocalMetricHypothesis::m_parent
mrpt::utils::safe_ptr< CHMTSLAM > m_parent
For quick access to our parent object.
Definition: CLocalMetricHypothesis.h:90
CParticleFilterCapable.h
mrpt::hmtslam::CLSLAMParticleData::~CLSLAMParticleData
virtual ~CLSLAMParticleData()
Definition: CLocalMetricHypothesis.h:54
mrpt::utils::CSerializable
The virtual base class which provides a unified interface for all persistent objects in MRPT.
Definition: CSerializable.h:39
HMTSLAM_IMPEXP
#define HMTSLAM_IMPEXP
Definition: hmtslam_impexp.h:81
DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE
#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...
Definition: CSerializable.h:170
mrpt::hmtslam::CLocalMetricHypothesis::m_log_w
double m_log_w
Log-weight of this hypothesis.
Definition: CLocalMetricHypothesis.h:99
mrpt::slam::CIncrementalMapPartitioner
This class can be used to make partitions on a map/graph build from observations taken at some poses/...
Definition: CIncrementalMapPartitioner.h:33
mrpt::hmtslam::CLocalMetricHypothesis::TRobotPosesPartitioning::lock
synch::CCriticalSection lock
CS to access the entire struct.
Definition: CLocalMetricHypothesis.h:109
mrpt::utils::list_searchable< CHMHMapNode::TNodeID >
mrpt::hmtslam::CLocalMetricHypothesis::m_SFs
std::map< TPoseID, mrpt::obs::CSensoryFrame > m_SFs
The SF gathered at each robot pose.
Definition: CLocalMetricHypothesis.h:95
DEFINE_SERIALIZABLE
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
Definition: CSerializable.h:147
mrpt::utils::safe_ptr
A wrapper class for pointers that can be safely copied with "=" operator without problems.
Definition: safe_pointers.h:64
CIncrementalMapPartitioner.h
mrpt::hmtslam::CLocalMetricHypothesis::m_pfAuxiliaryPFOptimal_estimatedProb
mrpt::math::CVectorDouble m_pfAuxiliaryPFOptimal_estimatedProb
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
Definition: CLocalMetricHypothesis.h:217
mrpt::poses::StdVector_CPose2D
mrpt::aligned_containers< CPose2D >::vector_t StdVector_CPose2D
Eigen aligment-compatible container.
Definition: CPose2D.h:265
CCriticalSection.h
mrpt::hmtslam::CHMHMapNode::TNodeID
mrpt::utils::TNodeID TNodeID
The type of the IDs of nodes.
Definition: CHMHMapNode.h:49
mrpt::utils::CStringList
A class for storing a list of text lines.
Definition: CStringList.h:32
mrpt::hmtslam::CLocalMetricHypothesis::m_maxLikelihood
std::vector< double > m_maxLikelihood
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
Definition: CLocalMetricHypothesis.h:221
mrpt::hmtslam::TMapPoseID2Pose3D
mrpt::aligned_containers< TPoseID, mrpt::poses::CPose3D >::map_t TMapPoseID2Pose3D
Definition: CLocalMetricHypothesis.h:31
CActionRobotMovement2D.h
mrpt::hmtslam::CLocalMetricHypothesis::m_areasPendingTBI
TNodeIDList m_areasPendingTBI
The list of area IDs waiting to be processed by the TBI (topological bayesian inference) engines to s...
Definition: CLocalMetricHypothesis.h:97
mrpt::bayes::CParticleFilter::TParticleFilterOptions
The configuration of a particle filter.
Definition: CParticleFilter.h:80
mrpt::hmtslam::CLocalMetricHypothesis::m_nodeIDmemberships
std::map< TPoseID, CHMHMapNode::TNodeID > m_nodeIDmemberships
The hybrid map node membership for each robot pose.
Definition: CLocalMetricHypothesis.h:94



Page generated by Doxygen 1.8.17 for MRPT 1.4.0 SVN: at Sat Jan 18 22:37:07 UTC 2020