MRPT  2.0.4
CLocalMetricHypothesis.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
12 
20 #include <map>
21 
22 #include <list>
23 #include <mutex>
24 
25 namespace mrpt
26 {
27 namespace poses
28 {
29 class CPose3DPDFParticles;
30 }
31 
32 namespace hmtslam
33 {
34 using TMapPoseID2Pose3D = std::map<TPoseID, mrpt::poses::CPose3D>;
35 
36 class CHMTSLAM;
38 
39 /** Auxiliary class used in mrpt::slam::CLocalMetricHypothesis for HMT-SLAM;
40  * this class keeps the data relative to each local metric particle ("a robot
41  * metric path hypothesis" and its associated metric map).
42  * \ingroup mrpt_hmtslam_grp
43  */
45 {
47 
48  public:
50  const mrpt::maps::TSetOfMetricMapInitializers* mapsInitializers =
51  nullptr)
52  : metricMaps(*mapsInitializers), robotPoses()
53  {
54  }
55 
58 };
59 
60 /** This class is used in HMT-SLAM to represent each of the Local Metric
61  * Hypotheses (LMHs).
62  * It has a set of particles representing the robot path in nearby poses.
63  * \sa CHMTSLAM, CLSLAM_RBPF_2DLASER
64  */
66  : public mrpt::bayes::CParticleFilterData<CLSLAMParticleData>,
68  CLocalMetricHypothesis,
69  mrpt::bayes::CParticleFilterData<CLSLAMParticleData>::CParticleList>,
71 {
72  friend class CLSLAM_RBPF_2DLASER;
73 
75 
76  public:
77  /** Constructor (Default param only used from STL classes) */
78  CLocalMetricHypothesis(CHMTSLAM* parent = nullptr);
79  ~CLocalMetricHypothesis() override;
80 
81  struct ThreadLocks
82  {
83  // Don't really copy mutexes
84  ThreadLocks() = default;
86  mutable std::mutex m_lock;
87  } threadLocks;
88  /** The unique ID of the hypothesis (Used for accessing
89  * mrpt::slam::CHierarchicalMHMap). */
91  /** For quick access to our parent object. */
93  /** The current robot pose (its global unique ID) for this hypothesis. */
95  // TNodeIDList m_neighbors; //!< The
96  // list of all areas sourronding
97  // the current one (this includes the current area itself).
98  /** The list of all areas sourronding the current one (this includes the
99  * current area itself). */
101  /** The hybrid map node membership for each robot pose. */
102  std::map<TPoseID, CHMHMapNode::TNodeID> m_nodeIDmemberships;
103  /** The SF gathered at each robot pose. */
104  std::map<TPoseID, mrpt::obs::CSensoryFrame> m_SFs;
105  /** The list of poseIDs waiting to be added to the graph partitioner, what
106  * happens in the LSLAM thread main loop. */
108  /** The list of area IDs waiting to be processed by the TBI (topological
109  * bayesian inference) engines to search for potential loop-closures. Set in
110  * CHMTSLAM::LSLAM_process_message_from_AA, read in */
112 
113  /** Log-weight of this hypothesis. */
114  double m_log_w;
115  /** The historic log-weights of the metric observations inserted in this
116  * LMH, for each particle. */
117  std::vector<std::map<TPoseID, double>> m_log_w_metric_history;
118  // std::map<TPoseID,double> m_log_w_topol_history; //!< The historic
119  // log-weights of the topological observations inserted in this LMH.
120 
121  /** Used in CLSLAM_RBPF_2DLASER */
123  /** Used in CLSLAM_RBPF_2DLASER */
125 
126  /** Used by AA thread */
128  {
129  TRobotPosesPartitioning() = default;
132  {
133  }
134  /** CS to access the entire struct. */
135  mutable std::mutex lock;
137  /** For the poses in "partitioner". */
138  std::map<uint32_t, TPoseID> idx2pose;
139 
140  /** Uses idx2pose to perform inverse searches. */
141  unsigned int pose2idx(const TPoseID& id) const;
142 
144 
145  /** Returns a 3D representation of the the current robot pose, all the poses
146  * in the auxiliary graph, and each of the areas they belong to.
147  * The metric maps are *not* included here for convenience, call
148  * m_metricMaps.getAs3DScene().
149  * The previous contents of "objs" will be discarded
150  */
152 
153  /** Returns the mean of each robot pose in this LMH, as computed from the
154  * set of particles.
155  * \sa getPathParticles, getRelativePose
156  */
157  void getMeans(TMapPoseID2Pose3D& outList) const;
158 
159  /** Returns the mean and covariance of each robot pose in this LMH, as
160  * computed from the set of particles.
161  * \sa getMeans, getPoseParticles
162  */
163  void getPathParticles(
164  std::map<TPoseID, mrpt::poses::CPose3DPDFParticles>& outList) const;
165 
166  /** Returns the mean and covariance of each robot pose in this LMH, as
167  * computed from the set of particles.
168  * \sa getMeans, getPathParticles
169  */
170  void getPoseParticles(
171  const TPoseID& poseID, mrpt::poses::CPose3DPDFParticles& outPDF) const;
172 
173  /** Returns the pose PDF of some pose relative to some other pose ID (both
174  * must be part of the the LMH).
175  * \sa getMeans, getPoseParticles
176  */
177  void getRelativePose(
178  const TPoseID& reference, const TPoseID& pose,
179  mrpt::poses::CPose3DPDFParticles& outPDF) const;
180 
181  /** Describes the LMH in text.
182  */
183  void dumpAsText(std::vector<std::string>& st) const;
184 
185  /** Change all coordinates to set a given robot pose as the new coordinate
186  * origin, and rebuild metric maps and change coords in the partitioning
187  * subsystem as well.
188  */
189  void changeCoordinateOrigin(const TPoseID& newOrigin);
190 
191  /** Rebuild the metric maps of all particles from the observations and their
192  * estimated poses. */
193  void rebuildMetricMaps();
194 
195  /** Rebuild the auxiliary metric maps in "m_robotPosesGraph" from the
196  * observations "m_SFs" and their estimated poses. */
197  // void rebuildSSOMatrix();
198 
199  /** Sets the number of particles to the initial number according to the PF
200  * options, and initialize them with no robot poses & empty metric maps.
201  */
202  void clearRobotPoses();
203 
204  /** Returns the i'th particle hypothesis for the current robot pose. */
205  const mrpt::poses::CPose3D* getCurrentPose(size_t particleIdx) const;
206 
207  /** Returns the i'th particle hypothesis for the current robot pose. */
208  mrpt::poses::CPose3D* getCurrentPose(size_t particleIdx);
209 
210  /** Removes a given area from the LMH:
211  * - The corresponding node in the HMT map is updated with the robot poses
212  *& SFs in the LMH.
213  * - Robot poses belonging to that area are removed from:
214  * - the particles.
215  * - the graph partitioner.
216  * - the list of SFs.
217  * - the list m_nodeIDmemberships.
218  * - m_neighbors is updated.
219  * - The weights of all particles are changed to remove the effects of the
220  *removed metric observations.
221  * - After calling this the metric maps should be updated.
222  * - This method internally calls updateAreaFromLMH
223  */
224  void removeAreaFromLMH(const CHMHMapNode::TNodeID areaID);
225 
226  /** The corresponding node in the HMT map is updated with the robot poses &
227  * SFs in the LMH: the poses are referenced to the area's reference poseID,
228  * such as that reference is at the origin.
229  * If eraseSFsFromLMH=true, the sensoryframes are moved rather than copied
230  * to the area, and removed from the LMH.
231  * \note The critical section m_map_cs is locked internally, unlock it
232  * before calling this.
233  */
234  void updateAreaFromLMH(
235  const CHMHMapNode::TNodeID areaID, bool eraseSFsFromLMH = false);
236 
237  protected:
238  /** @name Virtual methods for Particle Filter implementation (just a wrapper
239  interface, actually implemented in CHMTSLAM::m_LSLAM_method)
240  @{
241  */
242 
243  /** The PF algorithm implementation.
244  */
246  const mrpt::obs::CActionCollection* action,
247  const mrpt::obs::CSensoryFrame* observation,
249  override;
250 
251  /** The PF algorithm implementation. */
253  const mrpt::obs::CActionCollection* action,
254  const mrpt::obs::CSensoryFrame* observation,
256  override;
257  /** @}
258  */
259 
260  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
261  */
263 
264  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
265  */
266  mutable std::vector<double> m_maxLikelihood;
267 
268  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. */
269  mutable std::vector<mrpt::poses::CPose2D> m_movementDraws;
270 
271  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. */
272  mutable unsigned int m_movementDrawsIdx;
273 
274  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. */
275  mutable std::vector<mrpt::poses::CPose2D> m_movementDrawMaximumLikelihood;
276 
277 }; // End of class def.
278 
279 } // namespace hmtslam
280 } // namespace mrpt
std::vector< mrpt::poses::CPose2D > m_movementDraws
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
struct mrpt::hmtslam::CLocalMetricHypothesis::ThreadLocks threadLocks
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:566
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:44
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
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.
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)
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
Definition: CHMTSLAM.h:67
Declares a class for storing a collection of robot actions.
std::set< CHMHMapNode::TNodeID > TNodeIDSet
Definition: CHMHMapNode.h:143
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...
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...
std::map< TPoseID, mrpt::poses::CPose3D > TMapPoseID2Pose3D
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:51
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...
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...
const mrpt::poses::CPose3D * getCurrentPose(size_t particleIdx) const
Returns the i&#39;th particle hypothesis for the current robot pose.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
bool m_accumRobotMovementIsValid
Used in CLSLAM_RBPF_2DLASER.
Finds partitions in metric maps based on N-cut graph partition theory.
std::vector< mrpt::poses::CPose2D > m_movementDrawMaximumLikelihood
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
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:85
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:30
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...
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
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...
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::maps::CMultiMetricMap metricMaps



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020