MRPT  1.9.9
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-2019, 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  */
79  CLocalMetricHypothesis(CHMTSLAM* parent = nullptr);
80 
81  /** Destructor
82  */
83  ~CLocalMetricHypothesis() override;
84 
86  "Separate the serializable class from this code, so we don't have to "
87  "worry about copying locks")
88  struct ThreadLocks
89  {
90  // Don't really copy mutexes
91  ThreadLocks() = default;
92  ThreadLocks(const ThreadLocks&) {}
93  mutable std::mutex m_lock;
95  /** The unique ID of the hypothesis (Used for accessing
96  * mrpt::slam::CHierarchicalMHMap). */
98  /** For quick access to our parent object. */
100  /** The current robot pose (its global unique ID) for this hypothesis. */
102  // TNodeIDList m_neighbors; //!< The
103  // list of all areas sourronding
104  // the current one (this includes the current area itself).
105  /** The list of all areas sourronding the current one (this includes the
106  * current area itself). */
108  /** The hybrid map node membership for each robot pose. */
109  std::map<TPoseID, CHMHMapNode::TNodeID> m_nodeIDmemberships;
110  /** The SF gathered at each robot pose. */
111  std::map<TPoseID, mrpt::obs::CSensoryFrame> m_SFs;
112  /** The list of poseIDs waiting to be added to the graph partitioner, what
113  * happens in the LSLAM thread main loop. */
115  /** The list of area IDs waiting to be processed by the TBI (topological
116  * bayesian inference) engines to search for potential loop-closures. Set in
117  * CHMTSLAM::LSLAM_process_message_from_AA, read in */
119 
120  /** Log-weight of this hypothesis. */
121  double m_log_w;
122  /** The historic log-weights of the metric observations inserted in this
123  * LMH, for each particle. */
124  std::vector<std::map<TPoseID, double>> m_log_w_metric_history;
125  // std::map<TPoseID,double> m_log_w_topol_history; //!< The historic
126  // log-weights of the topological observations inserted in this LMH.
127 
128  /** Used in CLSLAM_RBPF_2DLASER */
130  /** Used in CLSLAM_RBPF_2DLASER */
132 
133  /** Used by AA thread */
135  {
136  TRobotPosesPartitioning() = default;
139  {
140  }
141  /** CS to access the entire struct. */
142  mutable std::mutex lock;
144  /** For the poses in "partitioner". */
145  std::map<uint32_t, TPoseID> idx2pose;
146 
147  /** Uses idx2pose to perform inverse searches. */
148  unsigned int pose2idx(const TPoseID& id) const;
149 
151 
152  /** Returns a 3D representation of the the current robot pose, all the poses
153  * in the auxiliary graph, and each of the areas they belong to.
154  * The metric maps are *not* included here for convenience, call
155  * m_metricMaps.getAs3DScene().
156  * The previous contents of "objs" will be discarded
157  */
159 
160  /** Returns the mean of each robot pose in this LMH, as computed from the
161  * set of particles.
162  * \sa getPathParticles, getRelativePose
163  */
164  void getMeans(TMapPoseID2Pose3D& 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, getPoseParticles
169  */
170  void getPathParticles(
171  std::map<TPoseID, mrpt::poses::CPose3DPDFParticles>& outList) const;
172 
173  /** Returns the mean and covariance of each robot pose in this LMH, as
174  * computed from the set of particles.
175  * \sa getMeans, getPathParticles
176  */
177  void getPoseParticles(
178  const TPoseID& poseID, mrpt::poses::CPose3DPDFParticles& outPDF) const;
179 
180  /** Returns the pose PDF of some pose relative to some other pose ID (both
181  * must be part of the the LMH).
182  * \sa getMeans, getPoseParticles
183  */
184  void getRelativePose(
185  const TPoseID& reference, const TPoseID& pose,
186  mrpt::poses::CPose3DPDFParticles& outPDF) const;
187 
188  /** Describes the LMH in text.
189  */
190  void dumpAsText(std::vector<std::string>& st) const;
191 
192  /** Change all coordinates to set a given robot pose as the new coordinate
193  * origin, and rebuild metric maps and change coords in the partitioning
194  * subsystem as well.
195  */
196  void changeCoordinateOrigin(const TPoseID& newOrigin);
197 
198  /** Rebuild the metric maps of all particles from the observations and their
199  * estimated poses. */
200  void rebuildMetricMaps();
201 
202  /** Rebuild the auxiliary metric maps in "m_robotPosesGraph" from the
203  * observations "m_SFs" and their estimated poses. */
204  // void rebuildSSOMatrix();
205 
206  /** Sets the number of particles to the initial number according to the PF
207  * options, and initialize them with no robot poses & empty metric maps.
208  */
209  void clearRobotPoses();
210 
211  /** Returns the i'th particle hypothesis for the current robot pose. */
212  const mrpt::poses::CPose3D* getCurrentPose(const size_t& particleIdx) const;
213 
214  /** Returns the i'th particle hypothesis for the current robot pose. */
215  mrpt::poses::CPose3D* getCurrentPose(const size_t& particleIdx);
216 
217  /** Removes a given area from the LMH:
218  * - The corresponding node in the HMT map is updated with the robot poses
219  *& SFs in the LMH.
220  * - Robot poses belonging to that area are removed from:
221  * - the particles.
222  * - the graph partitioner.
223  * - the list of SFs.
224  * - the list m_nodeIDmemberships.
225  * - m_neighbors is updated.
226  * - The weights of all particles are changed to remove the effects of the
227  *removed metric observations.
228  * - After calling this the metric maps should be updated.
229  * - This method internally calls updateAreaFromLMH
230  */
231  void removeAreaFromLMH(const CHMHMapNode::TNodeID areaID);
232 
233  /** The corresponding node in the HMT map is updated with the robot poses &
234  * SFs in the LMH: the poses are referenced to the area's reference poseID,
235  * such as that reference is at the origin.
236  * If eraseSFsFromLMH=true, the sensoryframes are moved rather than copied
237  * to the area, and removed from the LMH.
238  * \note The critical section m_map_cs is locked internally, unlock it
239  * before calling this.
240  */
241  void updateAreaFromLMH(
242  const CHMHMapNode::TNodeID areaID, bool eraseSFsFromLMH = false);
243 
244  protected:
245  /** @name Virtual methods for Particle Filter implementation (just a wrapper
246  interface, actually implemented in CHMTSLAM::m_LSLAM_method)
247  @{
248  */
249 
250  /** The PF algorithm implementation.
251  */
253  const mrpt::obs::CActionCollection* action,
254  const mrpt::obs::CSensoryFrame* observation,
256  override;
257 
258  /** The PF algorithm implementation. */
260  const mrpt::obs::CActionCollection* action,
261  const mrpt::obs::CSensoryFrame* observation,
263  override;
264  /** @}
265  */
266 
267  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
268  */
270 
271  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
272  */
273  mutable std::vector<double> m_maxLikelihood;
274 
275  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. */
276  mutable std::vector<mrpt::poses::CPose2D> m_movementDraws;
277 
278  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. */
279  mutable unsigned int m_movementDrawsIdx;
280 
281  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. */
282  mutable std::vector<mrpt::poses::CPose2D> m_movementDrawMaximumLikelihood;
283 
284 }; // End of class def.
285 
286 } // namespace hmtslam
287 } // namespace mrpt
std::vector< mrpt::poses::CPose2D > m_movementDraws
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
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...
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...
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: 4363012a5 Tue Nov 19 10:55:26 2019 +0100 at mar nov 19 11:00:13 CET 2019