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
25namespace 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:
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
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 */
142 const TPoseID &reference,
143 const TPoseID &pose,
144 mrpt::poses::CPose3DPDFParticles &outPDF ) const;
145
146 /** Describes the LMH in text.
147 */
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. */
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 */
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 */
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 */
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 */
202 const mrpt::obs::CActionCollection * action,
203 const mrpt::obs::CSensoryFrame * observation,
205
206 /** The PF algorithm implementation. */
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
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
#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...
std::deque< CParticleData > CParticleList
Use this type to refer to the list of particles m_particles.
mrpt::utils::TNodeID TNodeID
The type of the IDs of nodes.
Definition: CHMHMapNode.h:49
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
Definition: CHMTSLAM.h:60
Implements a 2D local SLAM method based on a RBPF over an occupancy grid map.
Definition: CHMTSLAM.h:513
Auxiliary class used in mrpt::slam::CLocalMetricHypothesis for HMT-SLAM; this class keeps the data re...
CLSLAMParticleData(const mrpt::maps::TSetOfMetricMapInitializers *mapsInitializers=NULL)
mrpt::maps::CMultiMetricMap metricMaps
This class is used in HMT-SLAM to represent each of the Local Metric Hypotheses (LMHs).
mrpt::poses::StdVector_CPose2D m_movementDrawMaximumLikelihood
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
const mrpt::poses::CPose3D * getCurrentPose(const size_t &particleIdx) const
Returns the i'th particle hypothesis for the current robot pose.
std::map< TPoseID, mrpt::obs::CSensoryFrame > m_SFs
The SF gathered at each robot pose.
TNodeIDList m_areasPendingTBI
The list of area IDs waiting to be processed by the TBI (topological bayesian inference) engines to s...
void rebuildMetricMaps()
Rebuild the metric maps of all particles from the observations and their estimated poses.
void dumpAsText(utils::CStringList &st) const
Describes the LMH in text.
CLocalMetricHypothesis(CHMTSLAM *parent=NULL)
Constructor (Default param only used from STL classes)
mrpt::poses::CPose3D * getCurrentPose(const size_t &particleIdx)
Returns the i'th particle hypothesis for the current robot pose.
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).
void getAs3DScene(mrpt::opengl::CSetOfObjectsPtr &objs) const
Returns a 3D representation of the the current robot pose, all the poses in the auxiliary graph,...
double m_log_w
Log-weight of this hypothesis.
synch::CCriticalSection m_lock
Critical section for threads signaling they are working with the LMH.
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...
THypothesisID m_ID
The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
mrpt::utils::safe_ptr< CHMTSLAM > m_parent
For quick access to our parent object.
void clearRobotPoses()
Rebuild the auxiliary metric maps in "m_robotPosesGraph" from the observations "m_SFs" and their esti...
TNodeIDSet m_neighbors
The list of all areas sourronding the current one (this includes the current area itself).
void removeAreaFromLMH(const CHMHMapNode::TNodeID areaID)
Removes a given area from the LMH:
mrpt::obs::CActionRobotMovement2D m_accumRobotMovement
Used in CLSLAM_RBPF_2DLASER.
bool m_accumRobotMovementIsValid
Used in CLSLAM_RBPF_2DLASER.
std::vector< double > m_maxLikelihood
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
void prediction_and_update_pfAuxiliaryPFOptimal(const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
The PF algorithm implementation.
mrpt::math::CVectorDouble m_pfAuxiliaryPFOptimal_estimatedProb
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
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...
mrpt::poses::StdVector_CPose2D m_movementDraws
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
void changeCoordinateOrigin(const TPoseID &newOrigin)
Change all coordinates to set a given robot pose as the new coordinate origin, and rebuild metric map...
std::map< TPoseID, CHMHMapNode::TNodeID > m_nodeIDmemberships
The hybrid map node membership for each robot pose.
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.
unsigned int m_movementDrawsIdx
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
TPoseID m_currentRobotPose
The current robot pose (its global unique ID) for this hypothesis.
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...
void getMeans(TMapPoseID2Pose3D &outList) const
Returns the mean of each robot pose in this LMH, as computed from the set of particles.
void prediction_and_update_pfOptimalProposal(const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
The PF algorithm implementation.
TPoseIDList m_posesPendingAddPartitioner
The list of poseIDs waiting to be added to the graph partitioner, what happens in the LSLAM thread ma...
This class stores any customizable set of metric maps.
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
Definition: types_math.h:65
Declares a class for storing a collection of robot actions.
Represents a probabilistic 2D movement of the robot mobile base.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:73
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
This class can be used to make partitions on a map/graph build from observations taken at some poses/...
This class provides simple critical sections functionality.
The virtual base class which provides a unified interface for all persistent objects in MRPT.
Definition: CSerializable.h:40
A class for storing a list of text lines.
Definition: CStringList.h:33
#define HMTSLAM_IMPEXP
mrpt::aligned_containers< TPoseID, mrpt::poses::CPose3D >::map_t TMapPoseID2Pose3D
std::set< CHMHMapNode::TNodeID > TNodeIDSet
Definition: CHMHMapNode.h:150
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
std::vector< TPoseID > TPoseIDList
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
mrpt::aligned_containers< CPose2D >::vector_t StdVector_CPose2D
Eigen aligment-compatible container.
Definition: CPose2D.h:265
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
std::map< TYPE1, TYPE2, std::less< TYPE1 >, Eigen::aligned_allocator< std::pair< const TYPE1, TYPE2 > > > map_t
The configuration of a particle filter.
synch::CCriticalSection lock
CS to access the entire struct.
unsigned int pose2idx(const TPoseID &id) const
Uses idx2pose to perform inverse searches.
std::map< uint32_t, TPoseID > idx2pose
For the poses in "partitioner".
A wrapper class for pointers that can be safely copied with "=" operator without problems.
Definition: safe_pointers.h:65



Page generated by Doxygen 1.9.4 for MRPT 1.4.0 SVN: at Sun Aug 14 11:34:44 UTC 2022