Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, 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 
13 
16 
21 
22 #include <list>
23 
24 namespace mrpt
25 {
26 namespace poses
27 {
28 class CPose3DPDFParticles;
29 }
30 
31 namespace hmtslam
32 {
34 
35 class CHMTSLAM;
37 
38 /** Auxiliary class used in mrpt::slam::CLocalMetricHypothesis for HMT-SLAM;
39  * this class keeps the data relative to each local metric particle ("a robot
40  * metric path hypothesis" and its associated metric map).
41  * \ingroup mrpt_hmtslam_grp
42  */
44 {
46 
47  public:
49  const mrpt::maps::TSetOfMetricMapInitializers* mapsInitializers =
50  nullptr)
51  : metricMaps(mapsInitializers), robotPoses()
52  {
53  }
54 
57 };
58 
59 /** This class is used in HMT-SLAM to represent each of the Local Metric
60  * 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>,
67  CLocalMetricHypothesis,
68  mrpt::bayes::CParticleFilterData<CLSLAMParticleData>::CParticleList>,
70 {
71  friend class CLSLAM_RBPF_2DLASER;
72 
74 
75  public:
76  /** Constructor (Default param only used from STL classes)
77  */
78  CLocalMetricHypothesis(CHMTSLAM* parent = nullptr);
79 
80  /** Destructor
81  */
83 
85  "Separate the serializable class from this code, so we don't have to "
86  "worry about copying locks")
87  struct ThreadLocks
88  {
89  // Don't really copy mutexes
90  ThreadLocks() {}
91  ThreadLocks(const ThreadLocks&) {}
92  mutable std::mutex m_lock;
94  /** The unique ID of the hypothesis (Used for accessing
95  * mrpt::slam::CHierarchicalMHMap). */
97  /** For quick access to our parent object. */
99  /** The current robot pose (its global unique ID) for this hypothesis. */
101  // TNodeIDList m_neighbors; //!< The
102  // list of all areas sourronding
103  // the current one (this includes the current area itself).
104  /** The list of all areas sourronding the current one (this includes the
105  * current area itself). */
107  /** The hybrid map node membership for each robot pose. */
108  std::map<TPoseID, CHMHMapNode::TNodeID> m_nodeIDmemberships;
109  /** The SF gathered at each robot pose. */
110  std::map<TPoseID, mrpt::obs::CSensoryFrame> m_SFs;
111  /** The list of poseIDs waiting to be added to the graph partitioner, what
112  * happens in the LSLAM thread main loop. */
114  /** The list of area IDs waiting to be processed by the TBI (topological
115  * bayesian inference) engines to search for potential loop-closures. Set in
116  * CHMTSLAM::LSLAM_process_message_from_AA, read in */
118 
119  /** Log-weight of this hypothesis. */
120  double m_log_w;
121  /** The historic log-weights of the metric observations inserted in this
122  * LMH, for each particle. */
123  std::vector<std::map<TPoseID, double>> m_log_w_metric_history;
124  // std::map<TPoseID,double> m_log_w_topol_history; //!< The historic
125  // log-weights of the topological observations inserted in this LMH.
126 
127  /** Used in CLSLAM_RBPF_2DLASER */
129  /** Used in CLSLAM_RBPF_2DLASER */
131 
132  /** Used by AA thread */
134  {
138  {
139  }
140  /** CS to access the entire struct. */
141  mutable std::mutex lock;
143  /** For the poses in "partitioner". */
144  std::map<uint32_t, TPoseID> idx2pose;
145 
146  /** Uses idx2pose to perform inverse searches. */
147  unsigned int pose2idx(const TPoseID& id) const;
148 
150 
151  /** Returns a 3D representation of the the current robot pose, all the poses
152  * in the auxiliary graph, and each of the areas they belong to.
153  * The metric maps are *not* included here for convenience, call
154  * m_metricMaps.getAs3DScene().
155  * The previous contents of "objs" will be discarded
156  */
158 
159  /** Returns the mean of each robot pose in this LMH, as computed from the
160  * set of particles.
161  * \sa getPathParticles, getRelativePose
162  */
163  void getMeans(TMapPoseID2Pose3D& outList) const;
164 
165  /** Returns the mean and covariance of each robot pose in this LMH, as
166  * computed from the set of particles.
167  * \sa getMeans, getPoseParticles
168  */
169  void getPathParticles(
170  std::map<TPoseID, mrpt::poses::CPose3DPDFParticles>& outList) const;
171 
172  /** Returns the mean and covariance of each robot pose in this LMH, as
173  * computed from the set of particles.
174  * \sa getMeans, getPathParticles
175  */
176  void getPoseParticles(
177  const TPoseID& poseID, mrpt::poses::CPose3DPDFParticles& outPDF) const;
178 
179  /** Returns the pose PDF of some pose relative to some other pose ID (both
180  * must be part of the the LMH).
181  * \sa getMeans, getPoseParticles
182  */
183  void getRelativePose(
184  const TPoseID& reference, const TPoseID& pose,
185  mrpt::poses::CPose3DPDFParticles& outPDF) const;
186 
187  /** Describes the LMH in text.
188  */
189  void dumpAsText(std::vector<std::string>& st) const;
190 
191  /** Change all coordinates to set a given robot pose as the new coordinate
192  * origin, and rebuild metric maps and change coords in the partitioning
193  * subsystem as well.
194  */
195  void changeCoordinateOrigin(const TPoseID& newOrigin);
196 
197  /** Rebuild the metric maps of all particles from the observations and their
198  * estimated poses. */
199  void rebuildMetricMaps();
200 
201  /** Rebuild the auxiliary metric maps in "m_robotPosesGraph" from the
202  * observations "m_SFs" and their estimated poses. */
203  // void rebuildSSOMatrix();
204 
205  /** Sets the number of particles to the initial number according to the PF
206  * options, and initialize them with no robot poses & empty metric maps.
207  */
208  void clearRobotPoses();
209 
210  /** Returns the i'th particle hypothesis for the current robot pose. */
211  const mrpt::poses::CPose3D* getCurrentPose(const size_t& particleIdx) const;
212 
213  /** Returns the i'th particle hypothesis for the current robot pose. */
214  mrpt::poses::CPose3D* getCurrentPose(const size_t& particleIdx);
215 
216  /** Removes a given area from the LMH:
217  * - The corresponding node in the HMT map is updated with the robot poses
218  *& SFs in the LMH.
219  * - Robot poses belonging to that area are removed from:
220  * - the particles.
221  * - the graph partitioner.
222  * - the list of SFs.
223  * - the list m_nodeIDmemberships.
224  * - m_neighbors is updated.
225  * - The weights of all particles are changed to remove the effects of the
226  *removed metric observations.
227  * - After calling this the metric maps should be updated.
228  * - This method internally calls updateAreaFromLMH
229  */
230  void removeAreaFromLMH(const CHMHMapNode::TNodeID areaID);
231 
232  /** The corresponding node in the HMT map is updated with the robot poses &
233  * SFs in the LMH: the poses are referenced to the area's reference poseID,
234  * such as that reference is at the origin.
235  * If eraseSFsFromLMH=true, the sensoryframes are moved rather than copied
236  * to the area, and removed from the LMH.
237  * \note The critical section m_map_cs is locked internally, unlock it
238  * before calling this.
239  */
240  void updateAreaFromLMH(
241  const CHMHMapNode::TNodeID areaID, bool eraseSFsFromLMH = false);
242 
243  protected:
244  /** @name Virtual methods for Particle Filter implementation (just a wrapper
245  interface, actually implemented in CHMTSLAM::m_LSLAM_method)
246  @{
247  */
248 
249  /** The PF algorithm implementation.
250  */
252  const mrpt::obs::CActionCollection* action,
253  const mrpt::obs::CSensoryFrame* observation,
255  override;
256 
257  /** The PF algorithm implementation. */
259  const mrpt::obs::CActionCollection* action,
260  const mrpt::obs::CSensoryFrame* observation,
262  override;
263  /** @}
264  */
265 
266  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
267  */
269 
270  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
271  */
272  mutable std::vector<double> m_maxLikelihood;
273 
274  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. */
276 
277  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. */
278  mutable unsigned int m_movementDrawsIdx;
279 
280  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. */
283 
284 }; // End of class def.
285 
286 } // End of namespace
287 } // End of namespace
288 
289 #endif
std::map< TPoseID, CHMHMapNode::TNodeID > m_nodeIDmemberships
The hybrid map node membership for each robot pose.
Implements a 2D local SLAM method based on a RBPF over an occupancy grid map.
Definition: CHMTSLAM.h:567
THypothesisID m_ID
The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
void dumpAsText(std::vector< std::string > &st) const
Describes the LMH in text.
mrpt::graphs::TNodeID TNodeID
The type of the IDs of nodes.
Definition: CHMHMapNode.h:47
mrpt::aligned_std_vector< mrpt::poses::CPose2D > m_movementDraws
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
std::vector< double > m_maxLikelihood
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
void getAs3DScene(mrpt::opengl::CSetOfObjects::Ptr &objs) const
Returns a 3D representation of the the current robot pose, all the poses in the auxiliary graph...
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:44
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...
double m_log_w
Log-weight of this hypothesis.
CLocalMetricHypothesis(CHMTSLAM *parent=nullptr)
Constructor (Default param only used from STL classes)
std::map< KEY, VALUE, std::less< KEY >, mrpt::aligned_allocator_cpp11< std::pair< const KEY, VALUE > >> aligned_std_map
std::vector< T, mrpt::aligned_allocator_cpp11< T > > aligned_std_vector
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
Definition: CHMTSLAM.h:68
Declares a class for storing a collection of robot actions.
std::set< CHMHMapNode::TNodeID > TNodeIDSet
Definition: CHMHMapNode.h:146
std::vector< TPoseID > TPoseIDList
mrpt::obs::CActionRobotMovement2D m_accumRobotMovement
Used in CLSLAM_RBPF_2DLASER.
Represents a probabilistic 2D movement of the robot mobile base.
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...
mrpt::aligned_std_map< TPoseID, mrpt::poses::CPose3D > TMapPoseID2Pose3D
std::map< uint32_t, TPoseID > idx2pose
For the poses in "partitioner".
TPoseIDList m_posesPendingAddPartitioner
The list of poseIDs waiting to be added to the graph partitioner, what happens in the LSLAM thread ma...
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:54
mrpt::math::CVectorDouble m_pfAuxiliaryPFOptimal_estimatedProb
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
unsigned int m_movementDrawsIdx
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
This class is used in HMT-SLAM to represent each of the Local Metric Hypotheses (LMHs).
A wrapper class for pointers that can be safely copied with "=" operator without problems.
Definition: safe_pointers.h:71
This template class declares the array of particles and its internal data, managing some memory-relat...
struct mrpt::hmtslam::CLocalMetricHypothesis::TRobotPosesPartitioning m_robotPosesGraph
std::map< TPoseID, mrpt::obs::CSensoryFrame > m_SFs
The SF gathered at each robot pose.
Auxiliary class used in mrpt::slam::CLocalMetricHypothesis for HMT-SLAM; this class keeps the data re...
mrpt::aligned_std_vector< mrpt::poses::CPose2D > m_movementDrawMaximumLikelihood
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
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)...
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
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.
Finds partitions in metric maps based on N-cut graph partition theory.
void getMeans(TMapPoseID2Pose3D &outList) const
Returns the mean of each robot pose in this LMH, as computed from the set of particles.
void removeAreaFromLMH(const CHMHMapNode::TNodeID areaID)
Removes a given area from the LMH:
void rebuildMetricMaps()
Rebuild the metric maps of all particles from the observations and their estimated poses...
mrpt::safe_ptr< CHMTSLAM > m_parent
For quick access to our parent object.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
The configuration of a particle filter.
void prediction_and_update_pfOptimalProposal(const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options) override
The PF algorithm implementation.
void changeCoordinateOrigin(const TPoseID &newOrigin)
Change all coordinates to set a given robot pose as the new coordinate origin, and rebuild metric map...
void clearRobotPoses()
Rebuild the auxiliary metric maps in "m_robotPosesGraph" from the observations "m_SFs" and their esti...
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:32
unsigned int pose2idx(const TPoseID &id) const
Uses idx2pose to perform inverse searches.
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 ref...
This class stores any customizable set of metric maps.
void prediction_and_update_pfAuxiliaryPFOptimal(const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options) override
The PF algorithm implementation.
TNodeIDList m_areasPendingTBI
The list of area IDs waiting to be processed by the TBI (topological bayesian inference) engines to s...
const mrpt::poses::CPose3D * getCurrentPose(const size_t &particleIdx) const
Returns the i&#39;th particle hypothesis for the current robot pose.
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...
TNodeIDSet m_neighbors
The list of all areas sourronding the current one (this includes the current area itself)...
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
A curiously recurring template pattern (CRTP) approach to providing the basic functionality of any CP...
CLSLAMParticleData(const mrpt::maps::TSetOfMetricMapInitializers *mapsInitializers=nullptr)
TPoseID m_currentRobotPose
The current robot pose (its global unique ID) for this hypothesis.
MRPT_TODO("Separate the serializable class from this code, so we don't have to " "worry about copying locks") struct ThreadLocks
mrpt::maps::CMultiMetricMap metricMaps



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019