Main MRPT website > C++ reference
MRPT logo
CLocalMetricHypothesis.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef CLocalMetricHypothesis_H
29 #define CLocalMetricHypothesis_H
30 
31 #include <mrpt/synch.h>
32 #include <mrpt/bayes/CParticleFilterCapable.h>
33 
34 #include <mrpt/hmtslam/HMT_SLAM_common.h>
35 #include <mrpt/hmtslam/CHMHMapNode.h>
36 
37 #include <mrpt/slam/CMultiMetricMap.h>
38 #include <mrpt/slam/CActionRobotMovement2D.h>
39 #include <mrpt/slam/CIncrementalMapPartitioner.h>
40 
41 #include <list>
42 
43 namespace mrpt
44 {
45  namespace opengl
46  {
47  struct CSetOfObjectsPtr;
48  }
49  namespace poses
50  {
51  class CPose3DPDFParticles;
52  }
53 
54  namespace hmtslam
55  {
56  using namespace mrpt::slam;
57 
58  class HMTSLAM_IMPEXP CHMTSLAM;
59  class HMTSLAM_IMPEXP CLSLAM_RBPF_2DLASER;
60 
61  DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CLSLAMParticleData, mrpt::utils::CSerializable, HMTSLAM_IMPEXP )
62  DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CLocalMetricHypothesis, mrpt::utils::CSerializable, HMTSLAM_IMPEXP )
63 
64  /** 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).
65  * \ingroup mrpt_hmtslam_grp
66  */
67  class HMTSLAM_IMPEXP CLSLAMParticleData : public mrpt::utils::CSerializable
68  {
69  // This must be added to any CSerializable derived class:
70  DEFINE_SERIALIZABLE( CLSLAMParticleData )
71 
72  public:
73  CLSLAMParticleData( const TSetOfMetricMapInitializers *mapsInitializers = NULL ) :
74  metricMaps( mapsInitializers ),
75  robotPoses()
76  {
77  }
78 
79  virtual ~CLSLAMParticleData()
80  {
81  robotPoses.clear();
82  }
83 
84  CMultiMetricMap metricMaps;
85  std::map<TPoseID,CPose3D> robotPoses;
86  };
87 
88 
89  /** This class is used in HMT-SLAM to represent each of the Local Metric Hypotheses (LMHs).
90  * It has a set of particles representing the robot path in nearby poses.
91  * \sa CHMTSLAM, CLSLAM_RBPF_2DLASER
92  */
93  class HMTSLAM_IMPEXP CLocalMetricHypothesis :
94  public bayes::CParticleFilterCapable, public bayes::CParticleFilterData<CLSLAMParticleData>,
95  public mrpt::utils::CSerializable
96  {
97  friend class HMTSLAM_IMPEXP CLSLAM_RBPF_2DLASER;
98 
99  // This must be added to any CSerializable derived class:
100  DEFINE_SERIALIZABLE( CLocalMetricHypothesis )
101 
102  public:
103  /** Constructor (Default param only used from STL classes)
104  */
105  CLocalMetricHypothesis( CHMTSLAM * parent = NULL );
106 
107  /** Destructor
108  */
109  ~CLocalMetricHypothesis();
110 
111  synch::CCriticalSection m_lock; //!< Critical section for threads signaling they are working with the LMH.
112  THypothesisID m_ID; //!< The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
113  safe_ptr<CHMTSLAM> m_parent; //!< For quick access to our parent object.
114  TPoseID m_currentRobotPose; //!< The current robot pose (its global unique ID) for this hypothesis.
115  //TNodeIDList m_neighbors; //!< The list of all areas sourronding the current one (this includes the current area itself).
116  TNodeIDSet m_neighbors; //!< The list of all areas sourronding the current one (this includes the current area itself).
117  std::map<TPoseID,CHMHMapNode::TNodeID> m_nodeIDmemberships; //!< The hybrid map node membership for each robot pose.
118  std::map<TPoseID,CSensoryFrame> m_SFs; //!< The SF gathered at each robot pose.
119  TPoseIDList m_posesPendingAddPartitioner; //!< The list of poseIDs waiting to be added to the graph partitioner, what happens in the LSLAM thread main loop.
120  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
121 
122  double m_log_w; //!< Log-weight of this hypothesis.
123  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.
124  //std::map<TPoseID,double> m_log_w_topol_history; //!< The historic log-weights of the topological observations inserted in this LMH.
125 
126  CActionRobotMovement2D m_accumRobotMovement; //!< Used in CLSLAM_RBPF_2DLASER
127  bool m_accumRobotMovementIsValid; //!< Used in CLSLAM_RBPF_2DLASER
128 
129  /** Used by AA thread */
130  struct TRobotPosesPartitioning
131  {
132  synch::CCriticalSection lock; //!< CS to access the entire struct.
133  CIncrementalMapPartitioner partitioner;
134  std::map<uint32_t,TPoseID> idx2pose; //!< For the poses in "partitioner".
135 
136  unsigned int pose2idx(const TPoseID &id) const; //!< Uses idx2pose to perform inverse searches.
137 
138  } m_robotPosesGraph;
139 
140  /** 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.
141  * The metric maps are *not* included here for convenience, call m_metricMaps.getAs3DScene().
142  * The previous contents of "objs" will be discarded
143  */
144  void getAs3DScene( mrpt::opengl::CSetOfObjectsPtr &objs ) const;
145 
146  /** Returns the mean of each robot pose in this LMH, as computed from the set of particles.
147  * \sa getPathParticles, getRelativePose
148  */
149  void getMeans( std::map< TPoseID, CPose3D > &outList ) const;
150 
151  /** Returns the mean and covariance of each robot pose in this LMH, as computed from the set of particles.
152  * \sa getMeans, getPoseParticles
153  */
154  void getPathParticles( std::map< TPoseID, CPose3DPDFParticles > &outList ) const;
155 
156  /** Returns the mean and covariance of each robot pose in this LMH, as computed from the set of particles.
157  * \sa getMeans, getPathParticles
158  */
159  void getPoseParticles( const TPoseID &poseID, CPose3DPDFParticles &outPDF ) const;
160 
161  /** Returns the pose PDF of some pose relative to some other pose ID (both must be part of the the LMH).
162  * \sa getMeans, getPoseParticles
163  */
164  void getRelativePose(
165  const TPoseID &reference,
166  const TPoseID &pose,
167  CPose3DPDFParticles &outPDF ) const;
168 
169  /** Describes the LMH in text.
170  */
171  void dumpAsText(utils::CStringList &st) const;
172 
173  /** 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.
174  */
175  void changeCoordinateOrigin( const TPoseID &newOrigin );
176 
177  /** Rebuild the metric maps of all particles from the observations and their estimated poses. */
178  void rebuildMetricMaps();
179 
180  /** Rebuild the auxiliary metric maps in "m_robotPosesGraph" from the observations "m_SFs" and their estimated poses. */
181  //void rebuildSSOMatrix();
182 
183  /** Sets the number of particles to the initial number according to the PF options, and initialize them with no robot poses & empty metric maps.
184  */
185  void clearRobotPoses();
186 
187  /** Returns the i'th particle hypothesis for the current robot pose. */
188  const CPose3D * getCurrentPose(const size_t &particleIdx) const;
189 
190  /** Returns the i'th particle hypothesis for the current robot pose. */
191  CPose3D * getCurrentPose(const size_t &particleIdx);
192 
193  /** Removes a given area from the LMH:
194  * - The corresponding node in the HMT map is updated with the robot poses & SFs in the LMH.
195  * - Robot poses belonging to that area are removed from:
196  * - the particles.
197  * - the graph partitioner.
198  * - the list of SFs.
199  * - the list m_nodeIDmemberships.
200  * - m_neighbors is updated.
201  * - The weights of all particles are changed to remove the effects of the removed metric observations.
202  * - After calling this the metric maps should be updated.
203  * - This method internally calls updateAreaFromLMH
204  */
205  void removeAreaFromLMH( const CHMHMapNode::TNodeID areaID );
206 
207  /** 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.
208  * If eraseSFsFromLMH=true, the sensoryframes are moved rather than copied to the area, and removed from the LMH.
209  * \note The critical section m_map_cs is locked internally, unlock it before calling this.
210  */
211  void updateAreaFromLMH(
212  const CHMHMapNode::TNodeID areaID,
213  bool eraseSFsFromLMH = false );
214 
215 
216  protected:
217 
218  /** @name Virtual methods for Particle Filter implementation (just a wrapper interface, actually implemented in CHMTSLAM::m_LSLAM_method)
219  @{
220  */
221 
222  /** The PF algorithm implementation.
223  */
224  void prediction_and_update_pfAuxiliaryPFOptimal(
225  const mrpt::slam::CActionCollection * action,
226  const mrpt::slam::CSensoryFrame * observation,
227  const bayes::CParticleFilter::TParticleFilterOptions &PF_options );
228 
229  /** The PF algorithm implementation. */
230  void prediction_and_update_pfOptimalProposal(
231  const mrpt::slam::CActionCollection * action,
232  const mrpt::slam::CSensoryFrame * observation,
233  const bayes::CParticleFilter::TParticleFilterOptions &PF_options );
234  /** @}
235  */
236 
237 
238  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
239  */
240  mutable vector_double m_pfAuxiliaryPFOptimal_estimatedProb;
241 
242  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
243  */
244  mutable std::vector<double> m_maxLikelihood;
245 
246  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
247  */
248  mutable std::vector<CPose2D,Eigen::aligned_allocator<CPose2D> > m_movementDraws;
249 
250  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
251  */
252  mutable unsigned int m_movementDrawsIdx;
253 
254  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
255  */
256  mutable StdVector_CPose2D m_movementDrawMaximumLikelihood;
257 
258 
259  /** The following implements:
260  * - CParticleFilterCapable::getW
261  * - CParticleFilterCapable::setW
262  * - CParticleFilterCapable::particlesCount
263  * - CParticleFilterCapable::normalizeWeights
264  * - CParticleFilterCapable::ESS
265  * - CParticleFilterCapable::performSubstitution
266  */
267  IMPLEMENT_PARTICLE_FILTER_CAPABLE(CLSLAMParticleData)
268 
269  }; // End of class def.
270 
271  } // End of namespace
272 } // End of namespace
273 
274 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013