Main MRPT website > C++ reference
MRPT logo
CIncrementalMapPartitioner.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 
29 #ifndef CINCREMENTALMAPPARTITIONER_H
30 #define CINCREMENTALMAPPARTITIONER_H
31 
32 #include <mrpt/utils/CDebugOutputCapable.h>
33 #include <mrpt/utils/CLoadableOptions.h>
34 
35 #include <mrpt/utils/stl_extensions.h>
36 
37 #include <mrpt/slam/CMultiMetricMap.h>
38 #include <mrpt/slam/CSimplePointsMap.h>
39 #include <mrpt/slam/CSimpleMap.h>
40 #include <mrpt/slam/CMultiMetricMap.h>
41 
42 #include <mrpt/slam/link_pragmas.h>
43 
44 namespace mrpt
45 {
46  namespace poses { class CPose3DPDF; }
47 namespace slam
48 {
49  DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CIncrementalMapPartitioner, mrpt::utils::CSerializable, SLAM_IMPEXP )
50 
51  /** This class can be used to make partitions on a map/graph build from
52  * observations taken at some poses/nodes.
53  * \ingroup mrpt_slam_grp
54  */
55  class SLAM_IMPEXP CIncrementalMapPartitioner : public utils::CDebugOutputCapable, public CSerializable
56  {
57  // This must be added to any CSerializable derived class:
58  DEFINE_SERIALIZABLE( CIncrementalMapPartitioner )
59 
60  public:
61  /** Constructor:
62  */
63  CIncrementalMapPartitioner( );
64 
65  /** Destructor:
66  */
67  virtual ~CIncrementalMapPartitioner();
68 
69  /** Initialization: Start of a new map, new internal matrices,...
70  */
71  void clear();
72 
73  /** Configuration of the algorithm:
74  */
75  struct SLAM_IMPEXP TOptions : public utils::CLoadableOptions
76  {
77  /** Sets default values at object creation
78  */
79  TOptions();
80 
81  /** Load parameters from configuration source
82  */
83  void loadFromConfigFile(
84  const mrpt::utils::CConfigFileBase &source,
85  const std::string &section);
86 
87  /** This method must display clearly all the contents of the structure in textual form, sending it to a CStream.
88  */
89  void dumpToTextStream(CStream &out) const;
90 
91  /** The partition threshold for bisection in range [0,2], default=1.0
92  */
93  float partitionThreshold;
94 
95  /** For the occupancy grid maps of each node, default=0.10
96  */
97  float gridResolution;
98 
99  /** Used in the computation of weights, default=0.20
100  */
101  float minDistForCorrespondence;
102 
103  /** Used in the computation of weights, default=2.0
104  */
105  float minMahaDistForCorrespondence;
106 
107  /** If set to true (default), 1 or 2 clusters will be returned. Default=false -> Autodetermine the number of partitions.
108  */
109  bool forceBisectionOnly;
110 
111  /** If set to true (default), adjacency matrix is computed from maps matching; otherwise, the method CObservation::likelihoodWith will be called directly from the SFs.
112  */
113  bool useMapMatching;
114 
115  /** This variable will be mapped to utils::CGraphPartitioner::DEBUG_SAVE_EIGENVECTOR_FILES.
116  */
117  bool debugSaveAllEigenvectors;
118 
119  /** If a partition leads to a cluster with less elements than this, it will be rejected even if had a good Ncut (default=1). */
120  int minimumNumberElementsEachCluster;
121 
122  } options;
123 
124  /** Add a new frame to the current graph: call this method each time a new observation
125  * is added to the map/graph, and whenever you want to update the partitions, call "updatePartitions"
126  * \param frame The sensed data
127  * \param robotPose An estimation of the robot global 2D pose.
128  * \return The index of the new pose in the internal list, which will be used to refer to the pose in the future.
129  * \sa updatePartitions
130  */
131  unsigned int addMapFrame( const CSensoryFramePtr &frame, const CPosePDFPtr &robotPose2D );
132 
133  /** Add a new frame to the current graph: call this method each time a new observation
134  * is added to the map/graph, and whenever you want to update the partitions, call "updatePartitions"
135  * \param frame The sensed data
136  * \param robotPose An estimation of the robot global 2D pose.
137  * \return The index of the new pose in the internal list, which will be used to refer to the pose in the future.
138  * \sa updatePartitions
139  */
140  unsigned int addMapFrame( const CSensoryFramePtr &frame, const CPose3DPDFPtr &robotPose3D );
141 
142  /** Add a new frame to the current graph: call this method each time a new observation
143  * is added to the map/graph, and whenever you want to update the partitions, call "updatePartitions"
144  * \param frame The sensed data
145  * \param robotPose An estimation of the robot global 2D pose.
146  * \return The index of the new pose in the internal list, which will be used to refer to the pose in the future.
147  * \sa updatePartitions
148  */
149  unsigned int addMapFrame( const CSensoryFrame &frame, const CPose3DPDF &robotPose3D );
150 
151  /** This method executed only the neccesary part of the partition to take
152  * into account the lastest added frames.
153  * \sa addMapFrame
154  */
155  void updatePartitions( std::vector<vector_uint> &partitions );
156 
157  /** It returns the nodes count currently in the internal map/graph.
158  */
159  unsigned int getNodesCount();
160 
161  /** Remove the stated nodes (0-based indexes) from the internal lists.
162  * If changeCoordsRef is true, coordinates are changed to leave the first node at (0,0,0).
163  */
164  void removeSetOfNodes(vector_uint indexesToRemove, bool changeCoordsRef = true);
165 
166  /** Returns a copy of the internal adjacency matrix. */
167  template <class MATRIX>
168  void getAdjacencyMatrix( MATRIX &outMatrix ) const { outMatrix = m_A; }
169 
170  /** Returns a const ref to the internal adjacency matrix. */
171  const CMatrixDouble & getAdjacencyMatrix( ) const { return m_A; }
172 
173  /** Read-only access to the sequence of Sensory Frames
174  */
175  const CSimpleMap* getSequenceOfFrames( ) const
176  {
177  return &m_individualFrames;
178  }
179 
180  /** Access to the sequence of Sensory Frames
181  */
182  CSimpleMap* getSequenceOfFrames( )
183  {
184  return &m_individualFrames;
185  }
186 
187  /** Mark all nodes for reconsideration in the next call to "updatePartitions", instead of considering just those affected by aditions of new arcs.
188  */
189  void markAllNodesForReconsideration();
190 
191  /** Change the coordinate origin of all stored poses, for consistency with future new poses to enter in the system. */
192  void changeCoordinatesOrigin( const CPose3D &newOrigin );
193 
194  /** Change the coordinate origin of all stored poses, for consistency with future new poses to enter in the system; the new origin is given by the index of the pose to become the new origin. */
195  void changeCoordinatesOriginPoseIndex( const unsigned &newOriginPose );
196 
197  /** Returns a 3D representation of the current state: poses & links between them.
198  * The previous contents of "objs" will be discarded
199  */
200  void getAs3DScene(
201  mrpt::opengl::CSetOfObjectsPtr &objs,
202  const std::map< uint32_t, int64_t > *renameIndexes = NULL
203  ) const;
204 
205  private:
206  /** El conjunto de los scans se guardan en un array:
207  */
208  CSimpleMap m_individualFrames;
209  std::deque<mrpt::slam::CMultiMetricMap> m_individualMaps;
210 
211 
212  /** Adjacency matrix
213  */
214  CMatrixD m_A;
215 
216  /** The last partition
217  */
218  std::vector<vector_uint> m_last_partition;
219 
220  /** This will be true after adding new observations, and before an "updatePartitions" is invoked.
221  */
222  bool m_last_last_partition_are_new_ones;
223 
224  /** La lista de nodos que hay que tener en cuenta en la proxima actualizacion:
225  */
226  std::vector<uint8_t> m_modified_nodes;
227 
228  }; // End of class def.
229 
230 
231  } // End of namespace
232 } // End of namespace
233 
234 #endif


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