Main MRPT website > C++ reference for MRPT 1.9.9
CIncrementalMapPartitioner.cpp
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 #include "slam-precomp.h" // Precompiled headers
11 
18 #include <mrpt/system/CTicTac.h>
23 #include <mrpt/opengl/CSphere.h>
25 
26 using namespace mrpt::slam;
27 using namespace mrpt::obs;
28 using namespace mrpt::maps;
29 using namespace mrpt::math;
30 using namespace mrpt::graphs;
31 using namespace mrpt::poses;
32 using namespace mrpt;
33 using namespace std;
34 
36 
37 
39  const CIncrementalMapPartitioner *parent,
40  const map_keyframe_t &kf1,
41  const map_keyframe_t &kf2,
42  const mrpt::poses::CPose3D &relPose2wrt1)
43 {
44  return kf1.metric_map->compute3DMatchingRatio(kf2.metric_map.get(), relPose2wrt1, parent->options.mrp);
45 }
47  const map_keyframe_t &kf1,
48  const map_keyframe_t &kf2,
49  const mrpt::poses::CPose3D &relPose2wrt1
50 )
51 {
52  return observationsOverlap(kf1.raw_observations, kf2.raw_observations, &relPose2wrt1);
53 }
54 
56 {
58  metricmap.push_back(def);
59 }
60 
62  const mrpt::config::CConfigFileBase& source, const string& section)
63 {
65 
66  MRPT_LOAD_CONFIG_VAR(partitionThreshold, double, source, section);
67  MRPT_LOAD_CONFIG_VAR(forceBisectionOnly, bool, source, section);
68  MRPT_LOAD_CONFIG_VAR(simil_method, enum, source, section);
70  minimumNumberElementsEachCluster, uint64_t, source, section);
72  "minDistForCorrespondence", double, mrp.maxDistForCorr,
73  source, section);
75  "minMahaDistForCorrespondence", double, mrp.maxMahaDistForCorr,
76  source, section);
77  MRPT_LOAD_CONFIG_VAR(maxKeyFrameDistanceToEval, uint64_t, source, section);
78 
79  mrpt::config::CConfigFilePrefixer cfp(source, section + std::string("."), "");
80  metricmap.loadFromConfigFile(cfp, "metricmap");
81  MRPT_TODO("Add link to example INI file");
82 
83  MRPT_END
84 }
85 
88  const std::string& s) const
89 {
90  MRPT_SAVE_CONFIG_VAR_COMMENT(partitionThreshold, "N-cut partition threshold [0,2]");
91  MRPT_SAVE_CONFIG_VAR_COMMENT(forceBisectionOnly, "Force bisection (true) or automatically determine number of partitions(false = default)");
92  MRPT_SAVE_CONFIG_VAR_COMMENT(simil_method, "Similarity method");
93  MRPT_SAVE_CONFIG_VAR_COMMENT(minimumNumberElementsEachCluster, "");
94  MRPT_SAVE_CONFIG_VAR_COMMENT(maxKeyFrameDistanceToEval, "Max KF ID distance");
95  c.write(s, "minDistForCorrespondence", mrp.maxDistForCorr, mrpt::config::MRPT_SAVE_NAME_PADDING(), mrpt::config::MRPT_SAVE_VALUE_PADDING());
96  c.write(s, "minMahaDistForCorrespondence", mrp.maxMahaDistForCorr, mrpt::config::MRPT_SAVE_NAME_PADDING(), mrpt::config::MRPT_SAVE_VALUE_PADDING());
97 
99  metricmap.saveToConfigFile(cfp, "metricmap");
100 }
101 
103 {
104  m_last_last_partition_are_new_ones = false;
105  m_A.setSize(0, 0);
106  m_individualFrames.clear(); // Free the map...
107  m_individualMaps.clear();
108  m_last_partition.clear(); // Delete last partitions
109 }
110 
112  const mrpt::obs::CSensoryFrame& frame,
113  const mrpt::poses::CPose3DPDF& robotPose)
114 {
115  MRPT_START
116 
117  const uint32_t new_id = m_individualMaps.size();
118  const size_t n = new_id + 1; // new size
119 
120  // Create new new metric map:
121  m_individualMaps.push_back(CMultiMetricMap::Create());
122  auto& newMetricMap = m_individualMaps.back();
123  newMetricMap->setListOfMaps(&options.metricmap);
124 
125  // Build robo-centric map for each keyframe:
126  frame.insertObservationsInto(newMetricMap.get());
127 
128  // Add tuple (pose,SF) to "simplemap":
129  m_individualFrames.insert(&robotPose, frame);
130 
131  // Expand the adjacency matrix (pads with 0)
132  m_A.setSize(n, n);
133 
134  ASSERT_(m_individualMaps.size() == n);
135  ASSERT_(m_individualFrames.size() == n);
136 
137  // Select method to evaluate similarity:
138  similarity_func_t sim_func;
139  using namespace std::placeholders; // for _1, _2 etc.
140  switch (options.simil_method)
141  {
143  sim_func = std::bind(&eval_similarity_metric_map_matching, this, _1, _2, _3);
144  break;
147  break;
148  case smCUSTOM_FUNCTION:
149  sim_func = m_sim_func;
150  break;
151  default:
152  THROW_EXCEPTION("Invalid value for `simil_method`");
153  };
154 
155  // Evaluate the similarity metric for the last row & column:
156  // (0:new_id, new_id) and (new_id, 0:new_id)
157  // ----------------------------------------------------------------
158  {
159  auto i = new_id;
160 
161  // KF "i":
162  map_keyframe_t map_i;
163  map_i.kf_id = i;
164  map_i.metric_map = m_individualMaps[i];
165  CPose3DPDF::Ptr posePDF_i;
166  m_individualFrames.get(i, posePDF_i, map_i.raw_observations);
167  auto pose_i = posePDF_i->getMeanVal();
168 
169  for (uint32_t j = 0; j < new_id; j++)
170  {
171  const auto id_diff = new_id - j;
172  double s_sym;
173  if (id_diff > options.maxKeyFrameDistanceToEval)
174  {
175  // skip evaluation
176  s_sym = .0;
177  }
178  else
179  {
180  // KF "j":
181  map_keyframe_t map_j;
182  CPose3DPDF::Ptr posePDF_j;
183  map_j.kf_id = j;
184  m_individualFrames.get(j, posePDF_j, map_j.raw_observations);
185  auto pose_j = posePDF_j->getMeanVal();
186  map_j.metric_map = m_individualMaps[j];
187 
188  auto relPose = pose_j - pose_i;
189 
190  // Evaluate similarity metric & make it symetric:
191  const auto s_ij = sim_func(map_i, map_j, relPose);
192  const auto s_ji = sim_func(map_j, map_i, relPose);
193  s_sym = 0.5*(s_ij + s_ji);
194  }
195  m_A(i, j) = m_A(j, i) = s_sym;
196  } // for j
197  } // i=n-1=new_id
198 
199  // Self-similatity: Not used
200  m_A(new_id, new_id) = 0;
201 
202  // If a partition has been already computed, add these new keyframes
203  // into a new partition on its own. When the user calls updatePartitions()
204  // all keyframes will be re-distributed according to the real similarity
205  // scores.
206  if (m_last_last_partition_are_new_ones)
207  {
208  // Insert into the "new_ones" partition:
209  m_last_partition[m_last_partition.size() - 1].push_back(n - 1);
210  }
211  else
212  {
213  // Add a new partition:
214  std::vector<uint32_t> dummyPart;
215  dummyPart.push_back(n - 1);
216  m_last_partition.emplace_back(dummyPart);
217 
218  // The last one is the new_ones partition:
219  m_last_last_partition_are_new_ones = true;
220  }
221 
222  return n - 1; // Index of the new node
223 
224  MRPT_END
225 }
226 
228  vector<std::vector<uint32_t>>& partitions)
229 {
230  MRPT_START
231 
232  partitions.clear();
234  m_A, partitions, options.partitionThreshold, true, true,
235  !options.forceBisectionOnly,
236  options.minimumNumberElementsEachCluster, false /* verbose */
237  );
238 
239  m_last_partition = partitions;
240  m_last_last_partition_are_new_ones = false;
241 
242  MRPT_END
243 }
244 
246 {
247  return m_individualFrames.size();
248 }
249 
251  std::vector<uint32_t> indexesToRemove, bool changeCoordsRef)
252 {
253  MRPT_START
254 
255  size_t nOld = m_A.cols();
256  size_t nNew = nOld - indexesToRemove.size();
257  size_t i, j;
258 
259  // Assure indexes are sorted:
260  std::sort(indexesToRemove.begin(), indexesToRemove.end());
261 
262  ASSERT_(nNew >= 1);
263 
264  // Build the vector with the nodes that REMAINS;
265  std::vector<uint32_t> indexesToStay;
266  indexesToStay.reserve(nNew);
267  for (i = 0; i < nOld; i++)
268  {
269  bool remov = false;
270  for (j = 0; !remov && j < indexesToRemove.size(); j++)
271  {
272  if (indexesToRemove[j] == i) remov = true;
273  }
274 
275  if (!remov) indexesToStay.push_back(i);
276  }
277 
278  ASSERT_(indexesToStay.size() == nNew);
279 
280  // Update the A matrix:
281  // ---------------------------------------------------
282  CMatrixDouble newA(nNew, nNew);
283  for (i = 0; i < nNew; i++)
284  for (j = 0; j < nNew; j++)
285  newA(i, j) = m_A(indexesToStay[i], indexesToStay[j]);
286 
287  // Substitute "A":
288  m_A = newA;
289 
290  // The last partitioning is all the nodes together:
291  // --------------------------------------------------
292  m_last_partition.resize(1);
293  m_last_partition[0].resize(nNew);
294  for (i = 0; i < nNew; i++) m_last_partition[0][i] = i;
295 
296  m_last_last_partition_are_new_ones = false;
297 
298  // The new sequence of maps:
299  // --------------------------------------------------
300  for (auto it = indexesToRemove.rbegin(); it != indexesToRemove.rend(); ++it)
301  {
302  auto itM = m_individualMaps.begin() + *it;
303  m_individualMaps.erase(itM); // Delete from list
304  }
305 
306  // The new sequence of localized SFs:
307  // --------------------------------------------------
308  for (auto it = indexesToRemove.rbegin(); it != indexesToRemove.rend(); ++it)
309  m_individualFrames.remove(*it);
310 
311  // Change coordinates reference of frames:
313  CPose3DPDF::Ptr posePDF;
314 
315  if (changeCoordsRef)
316  {
317  ASSERT_(m_individualFrames.size() > 0);
318  m_individualFrames.get(0, posePDF, SF);
319 
320  CPose3D p;
321  posePDF->getMean(p);
322  m_individualFrames.changeCoordinatesOrigin(p);
323  }
324 
325  // All done!
326  MRPT_END
327 }
328 
330  const CPose3D& newOrigin)
331 {
332  m_individualFrames.changeCoordinatesOrigin(newOrigin);
333 }
334 
336  unsigned int newOriginPose)
337 {
338  MRPT_START
339 
340  CPose3DPDF::Ptr pdf;
342  m_individualFrames.get(newOriginPose, pdf, sf);
343 
344  CPose3D p;
345  pdf->getMean(p);
346  changeCoordinatesOrigin(-p);
347 
348  MRPT_END
349 }
350 
353  const std::map<uint32_t, int64_t>* renameIndexes) const
354 {
355  objs->clear();
356  ASSERT_((int)m_individualFrames.size() == m_A.cols());
357 
358  auto gl_grid = opengl::CGridPlaneXY::Create();
359  objs->insert(gl_grid);
360  int bbminx = std::numeric_limits<int>::max(), bbminy = std::numeric_limits<int>::max();
361  int bbmaxx = -bbminx, bbmaxy = -bbminy;
362 
363  for (size_t i = 0; i < m_individualFrames.size(); i++)
364  {
365  CPose3DPDF::Ptr i_pdf;
366  CSensoryFrame::Ptr i_sf;
367  m_individualFrames.get(i, i_pdf, i_sf);
368 
369  CPose3D i_mean;
370  i_pdf->getMean(i_mean);
371 
372  mrpt::keep_min(bbminx, (int)floor(i_mean.x()));
373  mrpt::keep_min(bbminy, (int)floor(i_mean.y()));
374  mrpt::keep_max(bbmaxx, (int)ceil(i_mean.x()));
375  mrpt::keep_max(bbmaxy, (int)ceil(i_mean.y()));
376 
377  opengl::CSphere::Ptr i_sph =
378  mrpt::make_aligned_shared<opengl::CSphere>();
379  i_sph->setRadius(0.02f);
380  i_sph->setColor(0, 0, 1);
381 
382  if (!renameIndexes)
383  i_sph->setName(format("%u", static_cast<unsigned int>(i)));
384  else
385  {
387  renameIndexes->find(i);
388  ASSERT_(itName != renameIndexes->end());
389  i_sph->setName(
390  format("%lu", static_cast<unsigned long>(itName->second)));
391  }
392 
393  i_sph->enableShowName();
394  i_sph->setPose(i_mean);
395 
396  objs->insert(i_sph);
397 
398  // Arcs:
399  for (size_t j = i + 1; j < m_individualFrames.size(); j++)
400  {
401  CPose3DPDF::Ptr j_pdf;
402  CSensoryFrame::Ptr j_sf;
403  m_individualFrames.get(j, j_pdf, j_sf);
404 
405  CPose3D j_mean;
406  j_pdf->getMean(j_mean);
407 
408  float SSO_ij = m_A(i, j);
409 
410  if (SSO_ij > 0.01)
411  {
413  mrpt::make_aligned_shared<opengl::CSimpleLine>();
414  lin->setLineCoords(
415  i_mean.x(), i_mean.y(), i_mean.z(), j_mean.x(), j_mean.y(),
416  j_mean.z());
417 
418  lin->setColor(SSO_ij, 0, 1 - SSO_ij, SSO_ij * 0.6);
419  lin->setLineWidth(SSO_ij * 10);
420 
421  objs->insert(lin);
422  }
423  }
424  }
425  gl_grid->setPlaneLimits(bbminx, bbmaxx, bbminy, bbmaxy);
426  gl_grid->setGridFrequency(5);
427 }
428 
431 {
432  switch (version)
433  {
434  case 0:
435  case 1:
436  {
437  in >> m_individualFrames >> m_individualMaps >> m_A >>
438  m_last_partition >> m_last_last_partition_are_new_ones;
439  if (version == 0)
440  {
441  // field removed in v1
442  std::vector<uint8_t> old_modified_nodes;
443  in >> old_modified_nodes;
444  }
445  }
446  break;
447  default:
449  };
450 }
451 
455 {
456  out << m_individualFrames << m_individualMaps << m_A << m_last_partition
457  << m_last_last_partition_are_new_ones;
458 }
void updatePartitions(std::vector< std::vector< uint32_t >> &partitions)
Recalculate the map/graph partitions.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
#define MRPT_START
Definition: exceptions.h:262
size_t getNodesCount()
Get the total node count currently in the internal map/graph.
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()
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
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
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
int MRPT_SAVE_NAME_PADDING()
Default padding sizes for macros MRPT_SAVE_CONFIG_VAR_COMMENT(), etc.
Abstract graph and tree data structures, plus generic graph algorithms.
GLenum GLsizei n
Definition: glext.h:5074
double observationsOverlap(const mrpt::obs::CObservation *o1, const mrpt::obs::CObservation *o2, const mrpt::poses::CPose3D *pose_o2_wrt_o1=nullptr)
Estimates the "overlap" or "matching ratio" of two observations (range [0,1]), possibly taking into a...
STL namespace.
void keep_min(T &var, const K test_val)
If the second argument is below the first one, set the first argument to this lower value...
uint32_t addMapFrame(const mrpt::obs::CSensoryFrame &frame, const mrpt::poses::CPose3DPDF &robotPose3D)
Insert a new keyframe to the graph.
GLdouble s
Definition: glext.h:3676
void changeCoordinatesOrigin(const mrpt::poses::CPose3D &newOrigin)
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
static double eval_similarity_metric_map_matching(const CIncrementalMapPartitioner *parent, const map_keyframe_t &kf1, const map_keyframe_t &kf2, const mrpt::poses::CPose3D &relPose2wrt1)
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value...
void changeCoordinatesOriginPoseIndex(unsigned int newOriginPose)
The new origin is given by the index of the pose that is to become the new origin.
const GLubyte * c
Definition: glext.h:6313
Map keyframe, comprising raw observations and they as a metric map.
This namespace contains representation of robot actions and observations.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:54
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
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
int MRPT_SAVE_VALUE_PADDING()
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.
A wrapper for other CConfigFileBase-based objects that prefixes a given token to every key and/or sec...
GLsizei const GLchar ** string
Definition: glext.h:4101
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
#define MRPT_LOAD_CONFIG_VAR( variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
unsigned __int64 uint64_t
Definition: rptypes.h:50
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
static double eval_similarity_observation_overlap(const map_keyframe_t &kf1, const map_keyframe_t &kf2, const mrpt::poses::CPose3D &relPose2wrt1)
Finds partitions in metric maps based on N-cut graph partition theory.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
#define MRPT_TODO(x)
Definition: common.h:129
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
#define MRPT_END
Definition: exceptions.h:266
GLuint in
Definition: glext.h:7274
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
#define MRPT_LOAD_HERE_CONFIG_VAR( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
unsigned __int32 uint32_t
Definition: rptypes.h:47
bool insertObservationsInto(mrpt::maps::CMetricMap *theMap, const mrpt::poses::CPose3D *robotPose=nullptr) const
Insert all the observations in this SF into a metric map or any kind (see mrpt::maps::CMetricMap).
GLfloat GLfloat p
Definition: glext.h:6305
Algorithms for finding the min-normalized-cut of a weighted undirected graph.
static Ptr Create(Args &&... args)
Definition: CGridPlaneXY.h:34
const Scalar * const_iterator
Definition: eigen_plugins.h:27
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:42



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