MRPT  2.0.4
CIncrementalMapPartitioner.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-2020, 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 
10 #pragma once
11 
14 #include <mrpt/maps/CSimpleMap.h>
16 #include <mrpt/math/CMatrixD.h>
17 #include <mrpt/poses/poses_frwds.h>
20 #include <functional>
21 #include <limits>
22 
23 namespace mrpt::slam
24 {
25 /** For use in CIncrementalMapPartitioner
26  * \ingroup mrpt_slam_grp */
27 enum similarity_method_t : uint8_t
28 {
32 };
33 
34 /** Map keyframe, comprising raw observations and they as a metric map.
35  * For use in CIncrementalMapPartitioner
36  * \ingroup mrpt_slam_grp */
38 {
39  uint32_t kf_id{0};
42 };
43 
44 /** Type of similarity evaluator for map keyframes.
45  * For use in CIncrementalMapPartitioner
46  * \ingroup mrpt_slam_grp */
47 using similarity_func_t = std::function<double(
48  const map_keyframe_t& kf1, const map_keyframe_t& kf2,
49  const mrpt::poses::CPose3D& relPose2wrt1)>;
50 
51 /** Finds partitions in metric maps based on N-cut graph partition theory.
52  * \ingroup mrpt_slam_grp
53  */
56 {
57  // This must be added to any CSerializable derived class:
59 
60  public:
61  /** ctor */
62  CIncrementalMapPartitioner() : COutputLogger("CIncrementalMapPartitioner")
63  {
64  }
65 
66  /** Configuration parameters */
68  {
69  void loadFromConfigFile(
70  const mrpt::config::CConfigFileBase& source,
71  const std::string& section) override;
72  void saveToConfigFile(
74  const std::string& section) const override;
75 
76  /**!< N-cut partition threshold [0,2] (default=1) */
77  double partitionThreshold{1.0};
78 
79  /** These parameters are loaded/saved to config files
80  * with the prefix "mrp.{param_name}" */
82 
83  /* Force bisection (true) or automatically determine
84  * number of partitions (false=default).
85  */
86  bool forceBisectionOnly{false};
87 
88  /** Defines the method for determining the adjacency matrix values.
89  * \sa CIncrementalMapPartitioner::setSimilarityMethod()
90  */
92 
93  /** If a partition leads to a cluster with less elements than this, it
94  * will be rejected even if had a good Ncut (default=1).
95  */
97 
98  /** Type and parameters of metric map(s) to build for each keyframe.
99  * Parameters can be loaded from a config file from sections
100  * with the prefix of this "TOptions" section + ".metricmap".
101  * Default: a CSimplePointsMap */
103 
104  /** Maximum distance, in KF identifier numbers, to check for similarity.
105  * Default=Infinite. Can be used to constraint the wrong detection of
106  * clusters after loop closures but before correcting global poses. */
108  std::numeric_limits<uint64_t>::max()};
109 
110  TOptions();
111  };
112 
113  /** \name Main map partition API
114  * @{ */
115 
116  /** Algorithm parameters */
118 
119  /* Reset the internal state to an empty map */
120  void clear();
121 
122  /**\brief Insert a new keyframe to the graph.
123  *
124  * Call this method each time a new observation is added to the map/graph.
125  * Afterwards, call updatePartitions() to get the updated partitions.
126  *
127  * \param frame The sensed data
128  * \param robotPose An estimation of the robot global pose.
129  *
130  * \return The index of the new pose in the graph, which can be used to
131  * refer to this pose in the future.
132  *
133  * \sa updatePartitions
134  */
135  uint32_t addMapFrame(
136  const mrpt::obs::CSensoryFrame& frame,
137  const mrpt::poses::CPose3DPDF& robotPose3D);
138 
139  /** Recalculate the map/graph partitions. \sa addMapFrame() */
140  void updatePartitions(std::vector<std::vector<uint32_t>>& partitions);
141 
142  /**Get the total node count currently in the internal map/graph. */
143  size_t getNodesCount();
144 
145  /** Remove a list of keyframes, with indices as returned by addMapFrame()
146  * \param changeCoordsRef If true, coordinates are changed to leave the
147  * first node at (0,0,0).
148  */
149  void removeSetOfNodes(
150  std::vector<uint32_t> indexesToRemove, bool changeCoordsRef = true);
151 
152  /**\name Change Coordinates System
153  * \brief Change the coordinate origin of all stored poses
154  *
155  * Used for consistency with future new poses to enter in the system.
156  */
157  void changeCoordinatesOrigin(const mrpt::poses::CPose3D& newOrigin);
158  /**\brief The new origin is given by the index of the pose that is to
159  * become the new origin.
160  */
161  void changeCoordinatesOriginPoseIndex(unsigned int newOriginPose);
162 
163  /** Select the similarity method to use for newly inserted keyframes */
165  {
166  options.simil_method = method;
167  }
168 
169  /** Sets a custom function for the similarity of new keyframes */
171  {
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 = nullptr) 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  /** Read-only access to the sequence of Sensory Frames */
197  {
198  return &m_individualFrames;
199  }
200 
201  /** Access to the sequence of Sensory Frames */
203  {
204  return &m_individualFrames;
205  }
206  /** @} */
207 
208  private:
210  std::deque<mrpt::maps::CMultiMetricMap::Ptr> m_individualMaps;
211 
212  /** Adjacency matrix */
214 
215  /** The last partition */
216  std::vector<std::vector<uint32_t>> m_last_partition;
217 
218  /** This will be true after adding new observations, and before an
219  * "updatePartitions" is invoked. */
221 
223 
224 }; // End of class def.
225 } // namespace mrpt::slam
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 "CMatrixDynamic<double>".
Definition: CMatrixD.h:23
mrpt::math::CMatrixD m_A
Adjacency matrix.
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:32
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)
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...
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 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:51
mrpt::obs::CSensoryFrame::Ptr raw_observations
#define MRPT_ENUM_TYPE_END()
Definition: TEnumType.h:78
similarity_method_t
For use in CIncrementalMapPartitioner.
void saveToConfigFile(mrpt::config::CConfigFileBase &target, const std::string &section) const override
This method saves the options to a ".ini"-like file or memory-stored string list. ...
uint64_t maxKeyFrameDistanceToEval
Maximum distance, in KF identifier numbers, to check for similarity.
double partitionThreshold
!< N-cut partition threshold [0,2] (default=1)
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:85
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...
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:30
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
void getAs3DScene(mrpt::opengl::CSetOfObjects::Ptr &objs, const std::map< uint32_t, int64_t > *renameIndexes=nullptr) const
Return a 3D representation of the graph: poses & links between them.
#define MRPT_ENUM_TYPE_BEGIN(_ENUM_TYPE_WITH_NS)
Definition: TEnumType.h:62
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:39
const mrpt::maps::CSimpleMap * getSequenceOfFrames() const
Read-only access to the sequence of Sensory Frames.



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020