Main MRPT website > C++ reference for MRPT 1.9.9
CIncrementalMapPartitioner.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-2018, 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 
10 #pragma once
11 
16 #include <mrpt/maps/CSimpleMap.h>
18 #include <mrpt/poses/poses_frwds.h>
20 #include <functional>
21 #include <limits>
22 
23 namespace mrpt
24 {
25 namespace slam
26 {
27 
28 /** For use in CIncrementalMapPartitioner
29  * \ingroup mrpt_slam_grp */
31 {
35 };
36 
37 /** Map keyframe, comprising raw observations and they as a metric map.
38  * For use in CIncrementalMapPartitioner
39  * \ingroup mrpt_slam_grp */
41 {
45 };
46 
47 
48 /** Type of similarity evaluator for map keyframes.
49 * For use in CIncrementalMapPartitioner
50 * \ingroup mrpt_slam_grp */
51 using similarity_func_t = std::function<double(
52  const map_keyframe_t &kf1,
53  const map_keyframe_t &kf2,
54  const mrpt::poses::CPose3D &relPose2wrt1
55  )>;
56 
57 /** Finds partitions in metric maps based on N-cut graph partition theory.
58  * \ingroup mrpt_slam_grp
59  */
62 {
63  // This must be added to any CSerializable derived class:
65 
66  public:
67  /** ctor */
68  CIncrementalMapPartitioner() : COutputLogger("CIncrementalMapPartitioner") {}
69 
70  /** Configuration parameters */
72  {
73  void loadFromConfigFile(
75  const std::string& section) override;
77  const std::string& section) const;
78 
79  /**!< N-cut partition threshold [0,2] (default=1) */
80  double partitionThreshold{ 1.0 };
81 
82  /** These parameters are loaded/saved to config files
83  * with the prefix "mrp.{param_name}" */
85 
86  /* Force bisection (true) or automatically determine
87  * number of partitions (false=default).
88  */
89  bool forceBisectionOnly{ false };
90 
91  /** Defines the method for determining the adjacency matrix values.
92  * \sa CIncrementalMapPartitioner::setSimilarityMethod()
93  */
95 
96  /** If a partition leads to a cluster with less elements than this, it
97  * will be rejected even if had a good Ncut (default=1).
98  */
100 
101  /** Type and parameters of metric map(s) to build for each keyframe.
102  * Parameters can be loaded from a config file from sections
103  * with the prefix of this "TOptions" section + ".metricmap".
104  * Default: a CSimplePointsMap */
106 
107  /** Maximum distance, in KF identifier numbers, to check for similarity.
108  * Default=Infinite. Can be used to constraint the wrong detection of clusters
109  * after loop closures but before correcting global poses. */
110  uint64_t maxKeyFrameDistanceToEval{std::numeric_limits<uint64_t>::max()};
111 
112  TOptions();
113  };
114 
115  /** \name Main map partition API
116  * @{ */
117 
118  /** Algorithm parameters */
120 
121  /* Reset the internal state to an empty map */
122  void clear();
123 
124  /**\brief Insert a new keyframe to the graph.
125  *
126  * Call this method each time a new observation is added to the map/graph.
127  * Afterwards, call updatePartitions() to get the updated partitions.
128  *
129  * \param frame The sensed data
130  * \param robotPose An estimation of the robot global pose.
131  *
132  * \return The index of the new pose in the graph, which can be used to
133  * refer to this pose in the future.
134  *
135  * \sa updatePartitions
136  */
138  const mrpt::obs::CSensoryFrame& frame,
139  const mrpt::poses::CPose3DPDF& robotPose3D);
140 
141  /** Recalculate the map/graph partitions. \sa addMapFrame() */
142  void updatePartitions(std::vector<std::vector<uint32_t>>& partitions);
143 
144  /**Get the total node count currently in the internal map/graph. */
145  size_t getNodesCount();
146 
147  /** Remove a list of keyframes, with indices as returned by addMapFrame()
148  * \param changeCoordsRef If true, coordinates are changed to leave the first
149  * node at (0,0,0).
150  */
151  void removeSetOfNodes(
152  std::vector<uint32_t> indexesToRemove, bool changeCoordsRef = true);
153 
154  /**\name Change Coordinates System
155  * \brief Change the coordinate origin of all stored poses
156  *
157  * Used for consistency with future new poses to enter in the system.
158  */
159  void changeCoordinatesOrigin(const mrpt::poses::CPose3D& newOrigin);
160  /**\brief The new origin is given by the index of the pose that is to
161  * become the new origin.
162  */
163  void changeCoordinatesOriginPoseIndex(unsigned int newOriginPose);
164 
165  /** Select the similarity method to use for newly inserted keyframes */
167  options.simil_method = method;
168  }
169 
170  /** Sets a custom function for the similarity of new keyframes */
173  m_sim_func = func;
174  }
175  /** @} */
176 
177  /** \name Access API to internal graph data
178  * @{ */
179  /**Return a 3D representation of the graph: poses & links between them.
180  * The previous contents of "objs" will be discarded
181  */
182  void getAs3DScene(
184  const std::map<uint32_t, int64_t>* renameIndexes = NULL) const;
185 
186  /** Return a copy of the adjacency matrix. */
187  template <class MATRIX>
188  void getAdjacencyMatrix(MATRIX& outMatrix) const
189  {
190  outMatrix = m_A;
191  }
192 
193  /** Return a const ref to the internal adjacency matrix. */
195 
196  /** Read-only access to the sequence of Sensory Frames */
198  {
199  return &m_individualFrames;
200  }
201 
202  /** Access to the sequence of Sensory Frames */
204  {
205  return &m_individualFrames;
206  }
207  /** @} */
208 
209  private:
211  std::deque<mrpt::maps::CMultiMetricMap::Ptr> m_individualMaps;
212 
213  /** Adjacency matrix */
215 
216  /** The last partition */
217  std::vector<std::vector<uint32_t>> m_last_partition;
218 
219  /** This will be true after adding new observations, and before an
220  * "updatePartitions" is invoked. */
222 
224 
225 }; // End of class def.
226 }
227 } // End of namespace
228 
234 
MRPT_FILL_ENUM_MEMBER(mrpt::slam, smMETRIC_MAP_MATCHING)
std::deque< mrpt::maps::CMultiMetricMap::Ptr > m_individualMaps
void getAdjacencyMatrix(MATRIX &outMatrix) const
Return a copy of the adjacency matrix.
void updatePartitions(std::vector< std::vector< uint32_t >> &partitions)
Recalculate the map/graph partitions.
Parameters for CMetricMap::compute3DMatchingRatio()
size_t getNodesCount()
Get the total node count currently in the internal map/graph.
mrpt::maps::TMatchingRatioParams mrp
These parameters are loaded/saved to config files with the prefix "mrp.{param_name}".
This class is a "CSerializable" wrapper for "CMatrixTemplateNumeric<double>".
Definition: CMatrixD.h:24
mrpt::math::CMatrixD m_A
Adjacency matrix.
std::function< double(const map_keyframe_t &kf1, const map_keyframe_t &kf2, const mrpt::poses::CPose3D &relPose2wrt1)> similarity_func_t
Type of similarity evaluator for map keyframes.
void removeSetOfNodes(std::vector< uint32_t > indexesToRemove, bool changeCoordsRef=true)
Remove a list of keyframes, with indices as returned by addMapFrame()
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:35
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
mrpt::maps::CMultiMetricMap::Ptr metric_map
mrpt::maps::TSetOfMetricMapInitializers metricmap
Type and parameters of metric map(s) to build for each keyframe.
uint32_t addMapFrame(const mrpt::obs::CSensoryFrame &frame, const mrpt::poses::CPose3DPDF &robotPose3D)
Insert a new keyframe to the graph.
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
void changeCoordinatesOrigin(const mrpt::poses::CPose3D &newOrigin)
unsigned char uint8_t
Definition: rptypes.h:41
mrpt::maps::CSimpleMap * getSequenceOfFrames()
Access to the sequence of Sensory Frames.
std::vector< std::vector< uint32_t > > m_last_partition
The last partition.
This class allows loading and storing values and vectors of different types from a configuration text...
void changeCoordinatesOriginPoseIndex(unsigned int newOriginPose)
The new origin is given by the index of the pose that is to become the new origin.
Versatile class for consistent logging and management of output messages.
Map keyframe, comprising raw observations and they as a metric map.
const mrpt::math::CMatrixDouble & getAdjacencyMatrix() const
Return a const ref to the internal adjacency matrix.
similarity_method_t simil_method
Defines the method for determining the adjacency matrix values.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:54
void saveToConfigFile(mrpt::config::CConfigFileBase &target, const std::string &section) const
This method saves the options to a ".ini"-like file or memory-stored string list. ...
mrpt::obs::CSensoryFrame::Ptr raw_observations
void getAs3DScene(mrpt::opengl::CSetOfObjects::Ptr &objs, const std::map< uint32_t, int64_t > *renameIndexes=NULL) const
Return a 3D representation of the graph: poses & links between them.
#define MRPT_ENUM_TYPE_END()
Definition: TEnumType.h:74
GLsizei const GLchar ** string
Definition: glext.h:4101
similarity_method_t
For use in CIncrementalMapPartitioner.
uint64_t maxKeyFrameDistanceToEval
Maximum distance, in KF identifier numbers, to check for similarity.
double partitionThreshold
!< N-cut partition threshold [0,2] (default=1)
unsigned __int64 uint64_t
Definition: rptypes.h:50
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...
uint64_t minimumNumberElementsEachCluster
If a partition leads to a cluster with less elements than this, it will be rejected even if had a goo...
void setSimilarityMethod(similarity_func_t func)
Sets a custom function for the similarity of new keyframes.
Finds partitions in metric maps based on N-cut graph partition theory.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
COutputLogger()
Default class constructor.
bool m_last_last_partition_are_new_ones
This will be true after adding new observations, and before an "updatePartitions" is invoked...
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
void setSimilarityMethod(similarity_method_t method)
Select the similarity method to use for newly inserted keyframes.
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:32
#define MRPT_ENUM_TYPE_BEGIN(_ENUM_TYPE_WITH_NS)
Definition: TEnumType.h:58
unsigned __int32 uint32_t
Definition: rptypes.h:47
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:42
const mrpt::maps::CSimpleMap * getSequenceOfFrames() const
Read-only access to the sequence of Sensory Frames.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019