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::slam::CIncrementalMapPartitioner::TOptions
Configuration parameters.
Definition: CIncrementalMapPartitioner.h:67
mrpt::slam::CIncrementalMapPartitioner::TOptions::forceBisectionOnly
bool forceBisectionOnly
Definition: CIncrementalMapPartitioner.h:86
MRPT_ENUM_TYPE_END
#define MRPT_ENUM_TYPE_END()
Definition: TEnumType.h:78
mrpt::slam::CIncrementalMapPartitioner::getAs3DScene
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.
Definition: CIncrementalMapPartitioner.cpp:361
mrpt::slam::CIncrementalMapPartitioner::updatePartitions
void updatePartitions(std::vector< std::vector< uint32_t >> &partitions)
Recalculate the map/graph partitions.
Definition: CIncrementalMapPartitioner.cpp:237
mrpt::opengl::CSetOfObjects::Ptr
std::shared_ptr< mrpt::opengl ::CSetOfObjects > Ptr
Definition: CSetOfObjects.h:28
mrpt::slam::CIncrementalMapPartitioner::TOptions::mrp
mrpt::maps::TMatchingRatioParams mrp
These parameters are loaded/saved to config files with the prefix "mrp.{param_name}".
Definition: CIncrementalMapPartitioner.h:81
DEFINE_SERIALIZABLE
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
Definition: CSerializable.h:152
mrpt::slam::CIncrementalMapPartitioner::setSimilarityMethod
void setSimilarityMethod(similarity_method_t method)
Select the similarity method to use for newly inserted keyframes.
Definition: CIncrementalMapPartitioner.h:164
mrpt::obs::CSensoryFrame::Ptr
std::shared_ptr< mrpt::obs ::CSensoryFrame > Ptr
Definition: CSensoryFrame.h:53
CMultiMetricMap.h
mrpt::slam::smOBSERVATION_OVERLAP
@ smOBSERVATION_OVERLAP
Definition: CIncrementalMapPartitioner.h:30
mrpt::slam::smCUSTOM_FUNCTION
@ smCUSTOM_FUNCTION
Definition: CIncrementalMapPartitioner.h:31
mrpt::slam::CIncrementalMapPartitioner::m_individualFrames
mrpt::maps::CSimpleMap m_individualFrames
Definition: CIncrementalMapPartitioner.h:209
mrpt::slam::CIncrementalMapPartitioner::removeSetOfNodes
void removeSetOfNodes(std::vector< uint32_t > indexesToRemove, bool changeCoordsRef=true)
Remove a list of keyframes, with indices as returned by addMapFrame()
Definition: CIncrementalMapPartitioner.cpp:260
mrpt::slam::CIncrementalMapPartitioner::setSimilarityMethod
void setSimilarityMethod(similarity_func_t func)
Sets a custom function for the similarity of new keyframes.
Definition: CIncrementalMapPartitioner.h:170
mrpt::poses::CPose3DPDF
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually).
Definition: CPose3DPDF.h:39
mrpt::slam::CIncrementalMapPartitioner::m_A
mrpt::math::CMatrixD m_A
Adjacency matrix.
Definition: CIncrementalMapPartitioner.h:213
mrpt::slam::CIncrementalMapPartitioner::m_individualMaps
std::deque< mrpt::maps::CMultiMetricMap::Ptr > m_individualMaps
Definition: CIncrementalMapPartitioner.h:210
mrpt::slam::CIncrementalMapPartitioner::clear
void clear()
Definition: CIncrementalMapPartitioner.cpp:111
mrpt::maps::TMatchingRatioParams
Parameters for CMetricMap::compute3DMatchingRatio()
Definition: metric_map_types.h:64
mrpt::maps::CMultiMetricMap::Ptr
std::shared_ptr< mrpt::maps ::CMultiMetricMap > Ptr
Definition: CMultiMetricMap.h:122
mrpt::slam::CIncrementalMapPartitioner::getSequenceOfFrames
mrpt::maps::CSimpleMap * getSequenceOfFrames()
Access to the sequence of Sensory Frames.
Definition: CIncrementalMapPartitioner.h:202
MRPT_ENUM_TYPE_BEGIN
#define MRPT_ENUM_TYPE_BEGIN(_ENUM_TYPE_WITH_NS)
Definition: TEnumType.h:62
mrpt::slam::smMETRIC_MAP_MATCHING
@ smMETRIC_MAP_MATCHING
Definition: CIncrementalMapPartitioner.h:29
mrpt::slam::CIncrementalMapPartitioner::options
TOptions options
Algorithm parameters.
Definition: CIncrementalMapPartitioner.h:117
mrpt::maps::TSetOfMetricMapInitializers
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
Definition: TMetricMapInitializer.h:89
mrpt::slam::CIncrementalMapPartitioner::getSequenceOfFrames
const mrpt::maps::CSimpleMap * getSequenceOfFrames() const
Read-only access to the sequence of Sensory Frames.
Definition: CIncrementalMapPartitioner.h:196
mrpt::slam::CIncrementalMapPartitioner::TOptions::metricmap
mrpt::maps::TSetOfMetricMapInitializers metricmap
Type and parameters of metric map(s) to build for each keyframe.
Definition: CIncrementalMapPartitioner.h:102
mrpt::slam::CIncrementalMapPartitioner::TOptions::partitionThreshold
double partitionThreshold
!< N-cut partition threshold [0,2] (default=1)
Definition: CIncrementalMapPartitioner.h:77
mrpt::obs::CSensoryFrame
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:51
mrpt::slam::CIncrementalMapPartitioner::getAdjacencyMatrix
const mrpt::math::CMatrixDouble & getAdjacencyMatrix() const
Return a const ref to the internal adjacency matrix.
Definition: CIncrementalMapPartitioner.h:194
COutputLogger.h
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::slam::CIncrementalMapPartitioner::TOptions::saveToConfigFile
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.
Definition: CIncrementalMapPartitioner.cpp:85
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::slam::map_keyframe_t::raw_observations
mrpt::obs::CSensoryFrame::Ptr raw_observations
Definition: CIncrementalMapPartitioner.h:41
TEnumType.h
mrpt::slam::map_keyframe_t
Map keyframe, comprising raw observations and they as a metric map.
Definition: CIncrementalMapPartitioner.h:37
mrpt::config::CLoadableOptions
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
Definition: config/CLoadableOptions.h:26
mrpt::slam::similarity_func_t
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.
Definition: CIncrementalMapPartitioner.h:49
mrpt::serialization::CSerializable
The virtual base class which provides a unified interface for all persistent objects in MRPT.
Definition: CSerializable.h:30
mrpt::slam::CIncrementalMapPartitioner::m_last_last_partition_are_new_ones
bool m_last_last_partition_are_new_ones
This will be true after adding new observations, and before an "updatePartitions" is invoked.
Definition: CIncrementalMapPartitioner.h:220
mrpt::slam::CIncrementalMapPartitioner::TOptions::minimumNumberElementsEachCluster
uint64_t minimumNumberElementsEachCluster
If a partition leads to a cluster with less elements than this, it will be rejected even if had a goo...
Definition: CIncrementalMapPartitioner.h:96
CMatrixD.h
mrpt::slam::map_keyframe_t::kf_id
uint32_t kf_id
Definition: CIncrementalMapPartitioner.h:39
mrpt::slam::CIncrementalMapPartitioner::changeCoordinatesOrigin
void changeCoordinatesOrigin(const mrpt::poses::CPose3D &newOrigin)
Definition: CIncrementalMapPartitioner.cpp:339
mrpt::system::COutputLogger
Versatile class for consistent logging and management of output messages.
Definition: system/COutputLogger.h:117
mrpt::slam::CIncrementalMapPartitioner::getNodesCount
size_t getNodesCount()
Get the total node count currently in the internal map/graph.
Definition: CIncrementalMapPartitioner.cpp:255
mrpt::slam::CIncrementalMapPartitioner::TOptions::loadFromConfigFile
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.
Definition: CIncrementalMapPartitioner.cpp:59
mrpt::slam::CIncrementalMapPartitioner
Finds partitions in metric maps based on N-cut graph partition theory.
Definition: CIncrementalMapPartitioner.h:54
CLoadableOptions.h
poses_frwds.h
mrpt::slam::CIncrementalMapPartitioner::TOptions::simil_method
similarity_method_t simil_method
Defines the method for determining the adjacency matrix values.
Definition: CIncrementalMapPartitioner.h:91
mrpt::slam::CIncrementalMapPartitioner::addMapFrame
uint32_t addMapFrame(const mrpt::obs::CSensoryFrame &frame, const mrpt::poses::CPose3DPDF &robotPose3D)
Insert a new keyframe to the graph.
Definition: CIncrementalMapPartitioner.cpp:120
mrpt::slam::CIncrementalMapPartitioner::TOptions::TOptions
TOptions()
Definition: CIncrementalMapPartitioner.cpp:53
mrpt::slam::CIncrementalMapPartitioner::TOptions::maxKeyFrameDistanceToEval
uint64_t maxKeyFrameDistanceToEval
Maximum distance, in KF identifier numbers, to check for similarity.
Definition: CIncrementalMapPartitioner.h:107
mrpt::slam::CIncrementalMapPartitioner::m_last_partition
std::vector< std::vector< uint32_t > > m_last_partition
The last partition.
Definition: CIncrementalMapPartitioner.h:216
mrpt::maps::CSimpleMap
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:32
mrpt::slam::CIncrementalMapPartitioner::CIncrementalMapPartitioner
CIncrementalMapPartitioner()
ctor
Definition: CIncrementalMapPartitioner.h:62
mrpt::math::CMatrixD
This class is a "CSerializable" wrapper for "CMatrixDynamic<double>".
Definition: CMatrixD.h:23
MRPT_FILL_ENUM_MEMBER
MRPT_FILL_ENUM_MEMBER(mrpt::slam, smMETRIC_MAP_MATCHING)
CSimpleMap.h
CSimplePointsMap.h
mrpt::slam::CIncrementalMapPartitioner::m_sim_func
similarity_func_t m_sim_func
Definition: CIncrementalMapPartitioner.h:222
mrpt::slam
Definition: CMultiMetricMapPDF.h:26
mrpt::slam::similarity_method_t
similarity_method_t
For use in CIncrementalMapPartitioner.
Definition: CIncrementalMapPartitioner.h:27
mrpt::math::CMatrixDynamic< double >
mrpt::slam::CIncrementalMapPartitioner::changeCoordinatesOriginPoseIndex
void changeCoordinatesOriginPoseIndex(unsigned int newOriginPose)
The new origin is given by the index of the pose that is to become the new origin.
Definition: CIncrementalMapPartitioner.cpp:345
mrpt::slam::CIncrementalMapPartitioner::getAdjacencyMatrix
void getAdjacencyMatrix(MATRIX &outMatrix) const
Return a copy of the adjacency matrix.
Definition: CIncrementalMapPartitioner.h:188
mrpt::system::COutputLogger::COutputLogger
COutputLogger()
Default class constructor.
Definition: COutputLogger.cpp:70
mrpt::slam::map_keyframe_t::metric_map
mrpt::maps::CMultiMetricMap::Ptr metric_map
Definition: CIncrementalMapPartitioner.h:40



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sat Jun 27 14:00:59 UTC 2020