MRPT  1.9.9
CIncrementalMapPartitioner.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, 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 #include "slam-precomp.h" // Precompiled headers
11 
18 #include <mrpt/opengl/CSphere.h>
24 #include <mrpt/system/CTicTac.h>
25 #include <Eigen/Dense>
26 
27 using namespace mrpt::slam;
28 using namespace mrpt::obs;
29 using namespace mrpt::maps;
30 using namespace mrpt::math;
31 using namespace mrpt::graphs;
32 using namespace mrpt::poses;
33 using namespace mrpt;
34 using namespace std;
35 
37 
39  const CIncrementalMapPartitioner* parent, const map_keyframe_t& kf1,
40  const map_keyframe_t& kf2, const mrpt::poses::CPose3D& relPose2wrt1)
41 {
42  return kf1.metric_map->compute3DMatchingRatio(
43  kf2.metric_map.get(), relPose2wrt1, parent->options.mrp);
44 }
46  const map_keyframe_t& kf1, const map_keyframe_t& kf2,
47  const mrpt::poses::CPose3D& relPose2wrt1)
48 {
49  return observationsOverlap(
50  kf1.raw_observations, kf2.raw_observations, &relPose2wrt1);
51 }
52 
54 {
56  metricmap.push_back(def);
57 }
58 
60  const mrpt::config::CConfigFileBase& source, const string& section)
61 {
63 
64  MRPT_LOAD_CONFIG_VAR(partitionThreshold, double, source, section);
65  MRPT_LOAD_CONFIG_VAR(forceBisectionOnly, bool, source, section);
66  MRPT_LOAD_CONFIG_VAR(simil_method, enum, source, section);
68  minimumNumberElementsEachCluster, uint64_t, source, section);
70  "minDistForCorrespondence", double, mrp.maxDistForCorr, source,
71  section);
73  "minMahaDistForCorrespondence", double, mrp.maxMahaDistForCorr, source,
74  section);
75  MRPT_LOAD_CONFIG_VAR(maxKeyFrameDistanceToEval, uint64_t, source, section);
76 
78  source, section + std::string("."), "");
79  metricmap.loadFromConfigFile(cfp, "metricmap");
80  MRPT_TODO("Add link to example INI file");
81 
82  MRPT_END
83 }
84 
87 {
89  partitionThreshold, "N-cut partition threshold [0,2]");
91  forceBisectionOnly,
92  "Force bisection (true) or automatically determine number of "
93  "partitions(false = default)");
94  MRPT_SAVE_CONFIG_VAR_COMMENT(simil_method, "Similarity method");
95  MRPT_SAVE_CONFIG_VAR_COMMENT(minimumNumberElementsEachCluster, "");
97  maxKeyFrameDistanceToEval, "Max KF ID distance");
98  c.write(
99  s, "minDistForCorrespondence", mrp.maxDistForCorr,
102  c.write(
103  s, "minMahaDistForCorrespondence", mrp.maxMahaDistForCorr,
106 
108  metricmap.saveToConfigFile(cfp, "metricmap");
109 }
110 
112 {
113  m_last_last_partition_are_new_ones = false;
114  m_A.setSize(0, 0);
115  m_individualFrames.clear(); // Free the map...
116  m_individualMaps.clear();
117  m_last_partition.clear(); // Delete last partitions
118 }
119 
121  const mrpt::obs::CSensoryFrame& frame,
122  const mrpt::poses::CPose3DPDF& robotPose)
123 {
124  MRPT_START
125 
126  const uint32_t new_id = m_individualMaps.size();
127  const size_t n = new_id + 1; // new size
128 
129  // Create new new metric map:
130  m_individualMaps.push_back(CMultiMetricMap::Create());
131  auto& newMetricMap = m_individualMaps.back();
132  newMetricMap->setListOfMaps(options.metricmap);
133 
134  // Build robo-centric map for each keyframe:
135  frame.insertObservationsInto(newMetricMap.get());
136 
137  // Add tuple (pose,SF) to "simplemap":
138  m_individualFrames.insert(&robotPose, frame);
139 
140  // Expand the adjacency matrix (pads with 0)
141  m_A.setSize(n, n);
142 
143  ASSERT_(m_individualMaps.size() == n);
144  ASSERT_(m_individualFrames.size() == n);
145 
146  // Select method to evaluate similarity:
147  similarity_func_t sim_func;
148  using namespace std::placeholders; // for _1, _2 etc.
149  switch (options.simil_method)
150  {
152  sim_func = std::bind(
153  &eval_similarity_metric_map_matching, this, _1, _2, _3);
154  break;
157  break;
158  case smCUSTOM_FUNCTION:
159  sim_func = m_sim_func;
160  break;
161  default:
162  THROW_EXCEPTION("Invalid value for `simil_method`");
163  };
164 
165  // Evaluate the similarity metric for the last row & column:
166  // (0:new_id, new_id) and (new_id, 0:new_id)
167  // ----------------------------------------------------------------
168  {
169  auto i = new_id;
170 
171  // KF "i":
172  map_keyframe_t map_i;
173  map_i.kf_id = i;
174  map_i.metric_map = m_individualMaps[i];
175  CPose3DPDF::Ptr posePDF_i;
176  m_individualFrames.get(i, posePDF_i, map_i.raw_observations);
177  auto pose_i = posePDF_i->getMeanVal();
178 
179  for (uint32_t j = 0; j < new_id; j++)
180  {
181  const auto id_diff = new_id - j;
182  double s_sym;
183  if (id_diff > options.maxKeyFrameDistanceToEval)
184  {
185  // skip evaluation
186  s_sym = .0;
187  }
188  else
189  {
190  // KF "j":
191  map_keyframe_t map_j;
192  CPose3DPDF::Ptr posePDF_j;
193  map_j.kf_id = j;
194  m_individualFrames.get(j, posePDF_j, map_j.raw_observations);
195  auto pose_j = posePDF_j->getMeanVal();
196  map_j.metric_map = m_individualMaps[j];
197 
198  auto relPose = pose_j - pose_i;
199 
200  // Evaluate similarity metric & make it symetric:
201  const auto s_ij = sim_func(map_i, map_j, relPose);
202  const auto s_ji = sim_func(map_j, map_i, relPose);
203  s_sym = 0.5 * (s_ij + s_ji);
204  }
205  m_A(i, j) = m_A(j, i) = s_sym;
206  } // for j
207  } // i=n-1=new_id
208 
209  // Self-similatity: Not used
210  m_A(new_id, new_id) = 0;
211 
212  // If a partition has been already computed, add these new keyframes
213  // into a new partition on its own. When the user calls updatePartitions()
214  // all keyframes will be re-distributed according to the real similarity
215  // scores.
216  if (m_last_last_partition_are_new_ones)
217  {
218  // Insert into the "new_ones" partition:
219  m_last_partition[m_last_partition.size() - 1].push_back(n - 1);
220  }
221  else
222  {
223  // Add a new partition:
224  std::vector<uint32_t> dummyPart;
225  dummyPart.push_back(n - 1);
226  m_last_partition.emplace_back(dummyPart);
227 
228  // The last one is the new_ones partition:
229  m_last_last_partition_are_new_ones = true;
230  }
231 
232  return n - 1; // Index of the new node
233 
234  MRPT_END
235 }
236 
238  vector<std::vector<uint32_t>>& partitions)
239 {
240  MRPT_START
241 
242  partitions.clear();
244  m_A, partitions, options.partitionThreshold, true, true,
245  !options.forceBisectionOnly, options.minimumNumberElementsEachCluster,
246  false /* verbose */
247  );
248 
249  m_last_partition = partitions;
250  m_last_last_partition_are_new_ones = false;
251 
252  MRPT_END
253 }
254 
256 {
257  return m_individualFrames.size();
258 }
259 
261  std::vector<uint32_t> indexesToRemove, bool changeCoordsRef)
262 {
263  MRPT_START
264 
265  size_t nOld = m_A.cols();
266  size_t nNew = nOld - indexesToRemove.size();
267  size_t i, j;
268 
269  // Assure indexes are sorted:
270  std::sort(indexesToRemove.begin(), indexesToRemove.end());
271 
272  ASSERT_(nNew >= 1);
273 
274  // Build the vector with the nodes that REMAINS;
275  std::vector<uint32_t> indexesToStay;
276  indexesToStay.reserve(nNew);
277  for (i = 0; i < nOld; i++)
278  {
279  bool remov = false;
280  for (j = 0; !remov && j < indexesToRemove.size(); j++)
281  {
282  if (indexesToRemove[j] == i) remov = true;
283  }
284 
285  if (!remov) indexesToStay.push_back(i);
286  }
287 
288  ASSERT_(indexesToStay.size() == nNew);
289 
290  // Update the A matrix:
291  // ---------------------------------------------------
292  CMatrixDouble newA(nNew, nNew);
293  for (i = 0; i < nNew; i++)
294  for (j = 0; j < nNew; j++)
295  newA(i, j) = m_A(indexesToStay[i], indexesToStay[j]);
296 
297  // Substitute "A":
298  m_A = newA;
299 
300  // The last partitioning is all the nodes together:
301  // --------------------------------------------------
302  m_last_partition.resize(1);
303  m_last_partition[0].resize(nNew);
304  for (i = 0; i < nNew; i++) m_last_partition[0][i] = i;
305 
306  m_last_last_partition_are_new_ones = false;
307 
308  // The new sequence of maps:
309  // --------------------------------------------------
310  for (auto it = indexesToRemove.rbegin(); it != indexesToRemove.rend(); ++it)
311  {
312  auto itM = m_individualMaps.begin() + *it;
313  m_individualMaps.erase(itM); // Delete from list
314  }
315 
316  // The new sequence of localized SFs:
317  // --------------------------------------------------
318  for (auto it = indexesToRemove.rbegin(); it != indexesToRemove.rend(); ++it)
319  m_individualFrames.remove(*it);
320 
321  // Change coordinates reference of frames:
323  CPose3DPDF::Ptr posePDF;
324 
325  if (changeCoordsRef)
326  {
327  ASSERT_(m_individualFrames.size() > 0);
328  m_individualFrames.get(0, posePDF, SF);
329 
330  CPose3D p;
331  posePDF->getMean(p);
332  m_individualFrames.changeCoordinatesOrigin(p);
333  }
334 
335  // All done!
336  MRPT_END
337 }
338 
340  const CPose3D& newOrigin)
341 {
342  m_individualFrames.changeCoordinatesOrigin(newOrigin);
343 }
344 
346  unsigned int newOriginPose)
347 {
348  MRPT_START
349 
350  CPose3DPDF::Ptr pdf;
352  m_individualFrames.get(newOriginPose, pdf, sf);
353 
354  CPose3D p;
355  pdf->getMean(p);
356  changeCoordinatesOrigin(-p);
357 
358  MRPT_END
359 }
360 
363  const std::map<uint32_t, int64_t>* renameIndexes) const
364 {
365  objs->clear();
366  ASSERT_((int)m_individualFrames.size() == m_A.cols());
367 
368  auto gl_grid = opengl::CGridPlaneXY::Create();
369  objs->insert(gl_grid);
370  int bbminx = std::numeric_limits<int>::max(),
371  bbminy = std::numeric_limits<int>::max();
372  int bbmaxx = -bbminx, bbmaxy = -bbminy;
373 
374  for (size_t i = 0; i < m_individualFrames.size(); i++)
375  {
376  CPose3DPDF::Ptr i_pdf;
377  CSensoryFrame::Ptr i_sf;
378  m_individualFrames.get(i, i_pdf, i_sf);
379 
380  CPose3D i_mean;
381  i_pdf->getMean(i_mean);
382 
383  mrpt::keep_min(bbminx, (int)floor(i_mean.x()));
384  mrpt::keep_min(bbminy, (int)floor(i_mean.y()));
385  mrpt::keep_max(bbmaxx, (int)ceil(i_mean.x()));
386  mrpt::keep_max(bbmaxy, (int)ceil(i_mean.y()));
387 
388  opengl::CSphere::Ptr i_sph = std::make_shared<opengl::CSphere>();
389  i_sph->setRadius(0.02f);
390  i_sph->setColor(0, 0, 1);
391 
392  if (!renameIndexes)
393  i_sph->setName(format("%u", static_cast<unsigned int>(i)));
394  else
395  {
396  auto itName = renameIndexes->find(i);
397  ASSERT_(itName != renameIndexes->end());
398  i_sph->setName(
399  format("%lu", static_cast<unsigned long>(itName->second)));
400  }
401 
402  i_sph->enableShowName();
403  i_sph->setPose(i_mean);
404 
405  objs->insert(i_sph);
406 
407  // Arcs:
408  for (size_t j = i + 1; j < m_individualFrames.size(); j++)
409  {
410  CPose3DPDF::Ptr j_pdf;
411  CSensoryFrame::Ptr j_sf;
412  m_individualFrames.get(j, j_pdf, j_sf);
413 
414  CPose3D j_mean;
415  j_pdf->getMean(j_mean);
416 
417  float SSO_ij = m_A(i, j);
418 
419  if (SSO_ij > 0.01)
420  {
422  std::make_shared<opengl::CSimpleLine>();
423  lin->setLineCoords(
424  i_mean.x(), i_mean.y(), i_mean.z(), j_mean.x(), j_mean.y(),
425  j_mean.z());
426 
427  lin->setColor(SSO_ij, 0, 1 - SSO_ij, SSO_ij * 0.6);
428  lin->setLineWidth(SSO_ij * 10);
429 
430  objs->insert(lin);
431  }
432  }
433  }
434  gl_grid->setPlaneLimits(bbminx, bbmaxx, bbminy, bbmaxy);
435  gl_grid->setGridFrequency(5);
436 }
437 
439  mrpt::serialization::CArchive& in, uint8_t version)
440 {
441  switch (version)
442  {
443  case 0:
444  case 1:
445  {
446  in >> m_individualFrames >> m_individualMaps >> m_A >>
447  m_last_partition >> m_last_last_partition_are_new_ones;
448  if (version == 0)
449  {
450  // field removed in v1
451  std::vector<uint8_t> old_modified_nodes;
452  in >> old_modified_nodes;
453  }
454  }
455  break;
456  default:
458  };
459 }
460 
464 {
465  out << m_individualFrames << m_individualMaps << m_A << m_last_partition
466  << m_last_last_partition_are_new_ones;
467 }
void updatePartitions(std::vector< std::vector< uint32_t >> &partitions)
Recalculate the map/graph partitions.
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...
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:241
size_t getNodesCount()
Get the total node count currently in the internal map/graph.
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:67
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
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)
To be added to 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:5136
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.
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:3682
void changeCoordinatesOrigin(const mrpt::poses::CPose3D &newOrigin)
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
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:120
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.
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.
const GLubyte * c
Definition: glext.h:6406
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:51
mrpt::obs::CSensoryFrame::Ptr raw_observations
int MRPT_SAVE_VALUE_PADDING()
A wrapper for other CConfigFileBase-based objects that prefixes a given token to every key and/or sec...
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
GLsizei const GLchar ** string
Definition: glext.h:4116
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
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. ...
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...
#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...
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:54
#define MRPT_TODO(x)
Definition: common.h:129
unsigned __int64 uint64_t
Definition: glext.h:3457
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
#define MRPT_END
Definition: exceptions.h:245
GLuint in
Definition: glext.h:7391
GLsizei GLsizei GLchar * source
Definition: glext.h:4097
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
#define MRPT_LOAD_HERE_CONFIG_VAR( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
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.
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:6398
Finds the min-normalized-cut of a weighted undirected graph.
static Ptr Create(Args &&... args)
Definition: CGridPlaneXY.h:31
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:39



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ce444d842 Fri Dec 6 19:35:10 2019 +0100 at vie dic 6 19:45:12 CET 2019