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
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.
#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.
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:84
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...
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: 034c2ee2a Tue Aug 20 02:15:02 2019 +0200 at mar ago 20 02:20:10 CEST 2019