Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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/utils/CTicTac.h>
22 #include <mrpt/opengl/CSphere.h>
24 
25 using namespace mrpt::slam;
26 using namespace mrpt::obs;
27 using namespace mrpt::maps;
28 using namespace mrpt::math;
29 using namespace mrpt::graphs;
30 using namespace mrpt::poses;
31 using namespace mrpt::utils;
32 using namespace mrpt;
33 using namespace std;
34 
36 
37 /*---------------------------------------------------------------
38  Constructor
39  ---------------------------------------------------------------*/
41  COutputLogger("CIncrementalMapPartitioner"),
42  options(),
43  m_individualFrames(),
44  m_individualMaps(),
45  m_A(0,0),
46  m_last_partition(),
47  m_last_last_partition_are_new_ones(false),
48  m_modified_nodes()
49 {
50  clear();
51 }
52 
53 /*---------------------------------------------------------------
54  Destructor
55  ---------------------------------------------------------------*/
57 {
58  clear();
59 }
60 
61 /*---------------------------------------------------------------
62  Destructor
63  ---------------------------------------------------------------*/
65  partitionThreshold(1.0f),
66  gridResolution(0.10f),
67  minDistForCorrespondence(0.20f),
68  minMahaDistForCorrespondence(2.0f),
69  forceBisectionOnly(false),
70  useMapMatching(true),
71  minimumNumberElementsEachCluster(1) { }
72 
73 /*---------------------------------------------------------------
74  loadFromConfigFile
75  ---------------------------------------------------------------*/
78  const string &section)
79 {
81 
82  MRPT_LOAD_CONFIG_VAR(partitionThreshold, float,source,section);
83  MRPT_LOAD_CONFIG_VAR(gridResolution, float,source,section);
84  MRPT_LOAD_CONFIG_VAR(minDistForCorrespondence, float,source,section);
85  MRPT_LOAD_CONFIG_VAR(forceBisectionOnly, bool,source,section);
86  MRPT_LOAD_CONFIG_VAR(useMapMatching, bool,source,section);
87  MRPT_LOAD_CONFIG_VAR(minimumNumberElementsEachCluster, int, source,section);
88 
89  MRPT_END
90 }
91 
92 /*---------------------------------------------------------------
93  dumpToTextStream
94  ---------------------------------------------------------------*/
96 {
97  out.printf("\n----------- [CIncrementalMapPartitioner::TOptions] ------------ \n\n");
98 
99  out.printf("partitionThreshold = %f\n",partitionThreshold);
100  out.printf("gridResolution = %f\n",gridResolution);
101  out.printf("minDistForCorrespondence = %f\n",minDistForCorrespondence);
102  out.printf("forceBisectionOnly = %c\n",forceBisectionOnly ? 'Y':'N');
103  out.printf("useMapMatching = %c\n",useMapMatching ? 'Y':'N');
104  out.printf("minimumNumberElementsEachCluster = %i\n",minimumNumberElementsEachCluster);
105 }
106 
107 
108 /*---------------------------------------------------------------
109  clear
110  ---------------------------------------------------------------*/
112 {
114 
115  m_A.setSize(0,0);
116 
117  m_individualFrames.clear(); // Free the map...
118 
119  // Free individual maps:
120  //for (deque_serializable<mrpt::maps::CMultiMetricMap>::iterator it=m_individualMaps.begin();it!=m_individualMaps.end();++it) delete (*it);
121  m_individualMaps.clear();
122 
123  m_last_partition.clear(); // Delete last partitions
124  m_modified_nodes.clear(); // Delete modified nodes
125 }
126 
127 /*---------------------------------------------------------------
128  addMapFrame
129  ---------------------------------------------------------------*/
131  const CSensoryFramePtr &frame,
132  const CPosePDFPtr &robotPose)
133 {
134  MRPT_START
135  return addMapFrame(frame,CPose3DPDFPtr(CPose3DPDF::createFrom2D(*robotPose)));
136  MRPT_END
137 }
138 
139 
140 /*---------------------------------------------------------------
141  addMapFrame
142  ---------------------------------------------------------------*/
144  const CSensoryFramePtr &frame,
145  const CPose3DPDFPtr &robotPose)
146 {
147  size_t i=0,j=0,n=0;
148  CPose3D pose_i, pose_j, relPose;
149  CPose3DPDFPtr posePDF_i, posePDF_j;
150  CSensoryFramePtr sf_i, sf_j;
151  CMultiMetricMap *map_i=NULL,*map_j=NULL;
153  static CPose3D nullPose(0,0,0);
154 
155  // Create the maps:
156  TSetOfMetricMapInitializers mapInitializer;
157 
158  {
160  mapInitializer.push_back(def);
161  }
162 
163  {
165  mapInitializer.push_back(def);
166  }
167 
168  // Add new metric map to "m_individualMaps"
169  // --------------------------------------------
170  m_individualMaps.push_back(CMultiMetricMap());
171  CMultiMetricMap &newMetricMap = m_individualMaps.back();
172  newMetricMap.setListOfMaps(&mapInitializer);
173 
174  MRPT_START
175 
176  // Create the metric map:
177  // -----------------------------------------------------------------
178  ASSERT_(newMetricMap.m_pointsMaps.size()>0);
179  newMetricMap.m_pointsMaps[0]->insertionOptions.isPlanarMap = false; // true
180  newMetricMap.m_pointsMaps[0]->insertionOptions.minDistBetweenLaserPoints = 0.20f;
181  options.minDistForCorrespondence = max(options.minDistForCorrespondence,1.3f*newMetricMap.m_pointsMaps[0]->insertionOptions.minDistBetweenLaserPoints);
182 
186 
187  // JLBC,17/AGO/2006: "m_individualMaps" were created from the robot pose, but it is
188  // more convenient now to save them as the robot being at (0,0,0).
189 
190  //frame->insertObservationsInto(newMetricMap.m_pointsMaps[0]);
191  newMetricMap.m_pointsMaps[0]->copyFrom(* frame->buildAuxPointsMap<CPointsMap>(&newMetricMap.m_pointsMaps[0]->insertionOptions)); // Faster :-)
192 
193  // Insert just the VisualLandmarkObservations:
194  mrpt::maps::CLandmarksMap &lm = *newMetricMap.m_landmarksMap;
198  frame->insertObservationsInto(&lm);
199 
200  // Add to corresponding vectors:
201  m_individualFrames.insert(robotPose, frame);
202  // Already added to "m_individualMaps" above
203 
204  // Expand the adjacency matrix
205  // -----------------------------------------------------------------
206  n = m_A.getColCount();
207  n++;
208  m_A.setSize(n,n);
209 
210  ASSERT_(m_individualMaps.size() == n);
212 
213  // Adjust size of vector containing the modified nodes
214  // ---------------------------------------------------
215  // The new must be taken into account as well.
216  m_modified_nodes.push_back(true);
217 
218  // Methods to compute adjacency matrix:
219  // true: matching between maps
220  // false: matching between observations through "CObservation::likelihoodWith"
221  // ------------------------------------------------------------------------------
222  bool useMapOrSF = options.useMapMatching;
223 
224  // Calculate the new matches - put them in the matrix
225  // ----------------------------------------------------------------
226  //for (i=n-1;i<n;i++)
227  i=n-1; // Execute procedure until "i=n-1"; Last row/column is empty
228  {
229  // Get node "i":
230  m_individualFrames.get(i, posePDF_i, sf_i);
231  posePDF_i->getMean(pose_i);
232 
233  // And its points map:
234  map_i = &m_individualMaps[i];
235 
236  for (j=0;j<n-1;j++)
237  {
238  // Get node "j":
239  m_individualFrames.get(j, posePDF_j, sf_j);
240  posePDF_j->getMean(pose_j);
241 
242  relPose = pose_j - pose_i;
243 
244  // And its points map:
245  map_j = &m_individualMaps[j];
246 
247  // Compute matching ratio:
248  if (useMapOrSF) {
249  m_A(i,j) = map_i->compute3DMatchingRatio(map_j, relPose, mrp);
250  }
251  else {
252  //m_A(i,j) = sf_i->likelihoodWith(sf_j.pointer());
253  m_A(i,j) = observationsOverlap(sf_i, sf_j, &relPose);
254  }
255 
256  } // for j
257 
258  } // for i
259 
260 
261  for (i=0; i<n-1; i++) // Execute procedure until "i=n-1"; Last row/column is empty
262  {
263  // Get node "i":
264  m_individualFrames.get(i, posePDF_i, sf_i);
265  posePDF_i->getMean(pose_i);
266 
267  // And its points map:
268  map_i = &m_individualMaps[i];
269 
270  j=n-1; //for (j=n-1;j<n;j++)
271  {
272  // Get node "j":
273  m_individualFrames.get(j, posePDF_j, sf_j);
274  posePDF_j->getMean(pose_j);
275 
276  relPose = pose_j - pose_i;
277 
278  // And its points map:
279  map_j = &m_individualMaps[j];
280 
281  // Compute matching ratio:
282  if (useMapOrSF)
283  {
284  m_A(i,j) = map_i->compute3DMatchingRatio(map_j,CPose3D(relPose),mrp);
285  }
286  else
287  {
288  //m_A(i,j) = sf_i->likelihoodWith(sf_j.pointer());
289  m_A(i,j) = observationsOverlap(sf_i, sf_j, &relPose);
290  }
291  } // for j
292  } // for i
293 
294  // Self-similatity: Not used
295  m_A(n-1,n-1) = 0;
296 
297  // make matrix symetric
298  // -----------------------------------------------------------------
299  for (i=0;i<n;i++)
300  for (j=i+1;j<n;j++)
301  m_A(i,j) = m_A(j,i) = 0.5f * (m_A(i,j) + m_A(j,i));
302 
303  /* DEBUG: Save the matrix: * /
304  A.saveToTextFile("debug_matriz.txt",1);
305  / **/
306 
307  // Add the affected nodes to the list of modified ones
308  // -----------------------------------------------------------------
309  for (i=0;i<n;i++)
310  m_modified_nodes[i] = m_A(i,n-1) > 0;
311 
313  {
314  // Insert into the "new_ones" partition:
315  m_last_partition[m_last_partition.size()-1].push_back(n-1);
316  }
317  else
318  {
319  // Add a new partition:
320  vector_uint dummyPart;
321  dummyPart.push_back(n-1);
322  m_last_partition.push_back(dummyPart);
323 
324  // The last one is the new_ones partition:
326  }
327 
328  return n-1; // Index of the new node
329 
331  cout << "Unexpected runtime error:\n"; \
332  cout << "\tn=" << n << "\n"; \
333  cout << "\ti=" << i << "\n"; \
334  cout << "\tj=" << j << "\n"; \
335  cout << "\tmap_i=" << map_i << "\n"; \
336  cout << "\tmap_j=" << map_j << "\n"; \
337  cout << "relPose: "<< relPose << endl; \
338  cout << "map_i.size()=" << map_i->m_pointsMaps[0]->size() << "\n"; \
339  cout << "map_j.size()=" << map_j->m_pointsMaps[0]->size() << "\n"; \
340  map_i->m_pointsMaps[0]->save2D_to_text_file(string("debug_DUMP_map_i.txt")); \
341  map_j->m_pointsMaps[0]->save2D_to_text_file(string("debug_DUMP_map_j.txt")); \
342  m_A.saveToTextFile("debug_DUMP_exception_A.txt"); \
343  );
344 
345 }
346 
347 /*---------------------------------------------------------------
348  updatePartitions
349  ---------------------------------------------------------------*/
351  vector<vector_uint> &partitions)
352 {
353  MRPT_START
354 
355  unsigned int i,j;
356  unsigned int n_nodes;
357  unsigned int n_clusters_last;
358  vector_uint mods; // The list of nodes that will have been regrouped
359  vector_bool last_parts_are_mods;
360 
361  n_nodes = m_modified_nodes.size(); // total number of nodes (scans)
362  n_clusters_last = m_last_partition.size(); // Number of clusters in the last partition
363 
364  last_parts_are_mods.resize(n_clusters_last);
365 
366  // If a single scan of the cluster is affected, the whole cluster is affected
367  // -------------------------------------------------------------------
368  for (i=0;i<n_clusters_last;i++)
369  {
371 
372  // Recorrer esta particion:
373  last_parts_are_mods[i] = false;
374 
375 // for (j=0;j<p.size();j++)
376 // if (m_modified_nodes[ p[j] ])
377 // last_parts_are_mods[i] = true;
378 
379  last_parts_are_mods[i] = true;
380 
381  // If changed mark all the nodes
382  if (last_parts_are_mods[i])
383  for (j=0;j<p.size();j++)
384  m_modified_nodes[ p[j] ] = true;
385  }
386 
387  // How many nodes are going to be partitioned?
388  mods.clear();
389  for (i=0;i<n_nodes;i++)
390  if (m_modified_nodes[i])
391  mods.push_back(i);
392 
393  // printf("[%u nodes to be recomputed]", mods.size());
394 
395  if (mods.size()>0)
396  {
397 
398  // Construct submatrix of adjacencies only with the nodes that are going
399  // to be regrouped
400  // -------------------------------------------------------------------
401  CMatrix A_mods;
402  A_mods.setSize(mods.size(),mods.size());
403  for (i=0;i<mods.size();i++)
404  {
405  for (j=0;j<mods.size();j++)
406  {
407  A_mods(i,j) = m_A(mods[i],mods[j]);
408  }
409  }
410 
411  // Partitions of the modified nodes
412  vector<vector_uint> mods_parts;
413  mods_parts.clear();
414 
416  A_mods,
417  mods_parts,
419  true,
420  true,
423  false /* verbose */
424  );
425 
426  // Aggregate the results with the clusters that were not used and return them
427  // --------------------------------------------------------------------------
428  partitions.clear();
429 
430  // 1) Add the partitions that have not been modified
431  // -----------------------------------------------
432  for (i=0;i<m_last_partition.size();i++)
433  if (!last_parts_are_mods[i])
434  partitions.push_back(m_last_partition[i]);
435 
436 
437  // 2) Add the modified partitions
438  // WARNING: Translate the indices acordingly
439  // -----------------------------------------------
440  for (i=0;i<mods_parts.size();i++)
441  {
442  vector_uint v;
443  v.clear();
444  for (j=0;j<mods_parts[i].size();j++)
445  v.push_back(mods[mods_parts[i][j]]);
446 
447  partitions.push_back(v);
448  }
449  }
450 
451  // Update all nodes
452  for (i=0;i<n_nodes;i++)
453  m_modified_nodes[i] = false;
454 
455  // Save partition so that we take it into account in the next iteration
456  // ------------------------------------------------------------------------
457  size_t n = partitions.size();
458  m_last_partition.resize(n);
459  for (i=0;i<n;i++) m_last_partition[i] = partitions[i];
460 
462 
463  MRPT_END
464 }
465 
466 
467 
468 /*---------------------------------------------------------------
469  getNodesCount
470  ------------------------------------------------------------ ---*/
472 {
473  return m_individualFrames.size();
474 }
475 
476 /*---------------------------------------------------------------
477  removeSetOfNodes
478  ---------------------------------------------------------------*/
479 void CIncrementalMapPartitioner::removeSetOfNodes(vector_uint indexesToRemove, bool changeCoordsRef)
480 {
481  MRPT_START
482 
483  size_t nOld = m_A.getColCount();
484  size_t nNew = nOld - indexesToRemove.size();
485  size_t i,j;
486 
487  // Assure indexes are sorted:
488  std::sort(indexesToRemove.begin(), indexesToRemove.end());
489 
490  ASSERT_(nNew>=1);
491 
492  // Build the vector with the nodes that REMAINS;
493  vector_uint indexesToStay;
494  indexesToStay.reserve(nNew);
495  for (i=0;i<nOld;i++)
496  {
497  bool remov = false;
498  for (j=0;!remov && j<indexesToRemove.size();j++)
499  {
500  if (indexesToRemove[j]==i)
501  remov = true;
502  }
503 
504  if (!remov)
505  indexesToStay.push_back(i);
506  }
507 
508  ASSERT_(indexesToStay.size() == nNew);
509 
510  // Update the A matrix:
511  // ---------------------------------------------------
512  CMatrixDouble newA(nNew,nNew);
513  for (i=0;i<nNew;i++)
514  for (j=0;j<nNew;j++)
515  newA(i,j)=m_A(indexesToStay[i],indexesToStay[j]);
516 
517  // Substitute "A":
518  m_A = newA;
519 
520  // The last partitioning is all the nodes together:
521  // --------------------------------------------------
522  m_last_partition.resize(1);
523  m_last_partition[0].resize(nNew);
524  for (i=0;i<nNew;i++) m_last_partition[0][i] = i;
525 
527 
528  // The matrix "A" is supposed to be right, thus recomputing is not required:
529  m_modified_nodes.assign(nNew,false);
530 
531 
532 
533  // The new sequence of maps:
534  // --------------------------------------------------
535  vector_uint::reverse_iterator it;
536  for (it= indexesToRemove.rbegin(); it!=indexesToRemove.rend(); ++it)
537  {
539  // delete *itM; // Delete map
540  m_individualMaps.erase(itM); // Delete from list
541  }
542 
543  // The new sequence of localized SFs:
544  // --------------------------------------------------
545  for (it = indexesToRemove.rbegin(); it!=indexesToRemove.rend(); ++it)
547 
548  // Change coordinates reference of frames:
549  CSensoryFramePtr SF;
550  CPose3DPDFPtr posePDF;
551 
552  if (changeCoordsRef)
553  {
555  m_individualFrames.get(0, posePDF, SF);
556 
557  CPose3D p;
558  posePDF->getMean(p);
560  }
561 
562  // All done!
563 
564  MRPT_END
565 }
566 
567 /*---------------------------------------------------------------
568  markAllNodesForReconsideration
569  ---------------------------------------------------------------*/
571 {
573  m_last_partition.clear(); // No partitions in last step
574 
575  for (vector<uint8_t>::iterator it = m_modified_nodes.begin();it!=m_modified_nodes.end();++it)
576  *it = 1; //true;
577 }
578 
579 /*---------------------------------------------------------------
580  changeCoordinatesOrigin
581  ---------------------------------------------------------------*/
583 {
585 }
586 
587 /*---------------------------------------------------------------
588  changeCoordinatesOriginPoseIndex
589  ---------------------------------------------------------------*/
591 {
592  MRPT_START
593 
594  CPose3DPDFPtr pdf;
595  CSensoryFramePtr sf;
596  m_individualFrames.get(newOriginPose,pdf,sf);
597 
598  CPose3D p;
599  pdf->getMean(p);
601 
602  MRPT_END
603 }
604 
605 /*---------------------------------------------------------------
606  getAs3DScene
607  ---------------------------------------------------------------*/
609  mrpt::opengl::CSetOfObjectsPtr &objs,
610  const std::map<uint32_t,int64_t> *renameIndexes
611  ) const
612 {
613  objs->clear();
614  ASSERT_(m_individualFrames.size() == m_A.getColCount());
615 
616  objs->insert(opengl::CGridPlaneXY::Create(-100,100,-100,100,0,5));
617 
618  for (size_t i=0;i<m_individualFrames.size();i++)
619  {
620  CPose3DPDFPtr i_pdf;
621  CSensoryFramePtr i_sf;
622  m_individualFrames.get(i,i_pdf,i_sf);
623 
624  CPose3D i_mean;
625  i_pdf->getMean(i_mean);
626 
627  opengl::CSpherePtr i_sph = opengl::CSphere::Create();
628  i_sph->setRadius(0.02f);
629  i_sph->setColor(0,0,1);
630 
631  if (!renameIndexes)
632  i_sph->setName(format("%u",static_cast<unsigned int>(i)));
633  else
634  {
635  std::map<uint32_t,int64_t>::const_iterator itName = renameIndexes->find(i);
636  ASSERT_(itName != renameIndexes->end());
637  i_sph->setName(format("%lu",static_cast<unsigned long>(itName->second)));
638  }
639 
640  i_sph->enableShowName();
641  i_sph->setPose(i_mean);
642 
643  objs->insert(i_sph);
644 
645 
646  // Arcs:
647  for (size_t j=i+1;j<m_individualFrames.size();j++)
648  {
649  CPose3DPDFPtr j_pdf;
650  CSensoryFramePtr j_sf;
651  m_individualFrames.get(j,j_pdf,j_sf);
652 
653  CPose3D j_mean;
654  j_pdf->getMean(j_mean);
655 
656  float SSO_ij = m_A(i,j);
657 
658  if (SSO_ij>0.01)
659  {
660  opengl::CSimpleLinePtr lin = opengl::CSimpleLine::Create();
661  lin->setLineCoords(
662  i_mean.x(), i_mean.y(), i_mean.z(),
663  j_mean.x(), j_mean.y(), j_mean.z());
664 
665  lin->setColor(SSO_ij, 0, 1-SSO_ij, SSO_ij*0.6);
666  lin->setLineWidth(SSO_ij * 10);
667 
668  objs->insert(lin);
669  }
670  }
671 
672  }
673 
674 }
675 
676 /*---------------------------------------------------------------
677  readFromStream
678  ---------------------------------------------------------------*/
680 {
681  switch(version)
682  {
683  case 0:
684  {
687  >> m_A
690  >> m_modified_nodes;
691 
692  } break;
693  default:
695  };
696 }
697 
698 /*---------------------------------------------------------------
699  writeToStream
700  Implements the writing to a CStream capability of
701  CSerializable objects
702  ---------------------------------------------------------------*/
704 {
705  if (version)
706  *version = 0;
707  else
708  {
709  out << m_individualFrames
711  << m_A
714  << m_modified_nodes;
715  }
716 }
717 
718 /*---------------------------------------------------------------
719  addMapFrame
720  ---------------------------------------------------------------*/
721 unsigned int CIncrementalMapPartitioner::addMapFrame(const CSensoryFrame &frame, const CPose3DPDF &robotPose3D)
722 {
723  return addMapFrame(
724  CSensoryFramePtr(new CSensoryFrame(frame)),
725  CPose3DPDFPtr(robotPose3D.duplicateGetSmartPtr())
726  );
727 }
float partitionThreshold
The partition threshold for bisection in range [0,2], default=1.0.
std::deque< mrpt::maps::CMultiMetricMap > m_individualMaps
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
Parameters for CMetricMap::compute3DMatchingRatio()
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
bool insert_SIFTs_from_stereo_images
If set to true (default), the insertion of a CObservationStereoImages in the map will insert SIFT 3D ...
std::vector< uint32_t > vector_uint
Definition: types_simple.h:28
mrpt::math::CMatrixD m_A
Adjacency matrix.
float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const MRPT_OVERRIDE
See the definition in the base class: Calls in this class become a call to every single map in this s...
double SLAM_IMPEXP observationsOverlap(const mrpt::obs::CObservation *o1, const mrpt::obs::CObservation *o2, const mrpt::poses::CPose3D *pose_o2_wrt_o1=NULL)
Estimates the "overlap" or "matching ratio" of two observations (range [0,1]), possibly taking into a...
#define MRPT_END_WITH_CLEAN_UP(stuff)
unsigned int getNodesCount()
Get the total node count currently in the internal map/graph.
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:39
void markAllNodesForReconsideration()
Mark all nodes for reconsideration in the next call to "updatePartitions", instead of considering jus...
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
bool useMapMatching
If set to true (default), adjacency matrix is computed from maps matching; otherwise, the method CObservation::likelihoodWith will be called directly from the SFs.
Abstract graph and tree data structures, plus generic graph algorithms.
void updatePartitions(std::vector< vector_uint > &partitions)
This method executed only the neccesary part of the partition to take into account the lastest added ...
GLenum GLsizei n
Definition: glext.h:4618
Scalar * iterator
Definition: eigen_plugins.h:23
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
STL namespace.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
void getAs3DScene(mrpt::opengl::CSetOfObjectsPtr &objs, const std::map< uint32_t, int64_t > *renameIndexes=NULL) const
Return a 3D representation of the current state: poses & links between them.
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
void removeSetOfNodes(vector_uint indexesToRemove, bool changeCoordsRef=true)
Remove the stated nodes (0-based indexes) from the internal lists.
static CSpherePtr Create()
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:113
void changeCoordinatesOrigin(const mrpt::poses::CPose3D &newOrigin)
This class allows loading and storing values and vectors of different types from a configuration text...
std::vector< bool > vector_bool
A type for passing a vector of bools.
Definition: types_simple.h:29
float maxDistForCorr
(Default: 0.10f) The minimum distance between 2 non-probabilistic map elements for counting them as a...
bool insert_SIFTs_from_monocular_images
If set to true (default), the insertion of a CObservationImage in the map will insert SIFT 3D feature...
void changeCoordinatesOriginPoseIndex(const unsigned &newOriginPose)
The new origin is given by the index of the pose that is to become the new origin.
void readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
A class for storing a map of 3D probabilistic landmarks.
#define MRPT_END
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
A list of TMatchingPair.
Definition: TMatchingPair.h:78
ProxySelectorContainerByClass< mrpt::maps::CLandmarksMapPtr, TListMaps > m_landmarksMap
Proxy that looks like a smart pointer to the first matching object in maps.
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...
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
mrpt::slam::CIncrementalMapPartitioner::TOptions options
int version
Definition: mrpt_jpeglib.h:898
size_t size() const
Returns the count of pairs (pose,sensory data)
Definition: CSimpleMap.cpp:67
unsigned int addMapFrame(const mrpt::obs::CSensoryFramePtr &frame, const mrpt::poses::CPosePDFPtr &robotPose2D)
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
mrpt::utils::CObjectPtr duplicateGetSmartPtr() const
Returns a copy of the object, indepently of its class, as a smart pointer (the newly created object w...
Definition: CObject.h:162
mrpt::maps::CLandmarksMap::TInsertionOptions insertionOptions
void push_back(const MAP_DEFINITION &o)
std::vector< vector_uint > m_last_partition
The last partition.
void get(size_t index, mrpt::poses::CPose3DPDFPtr &out_posePDF, mrpt::obs::CSensoryFramePtr &out_SF) const
Access to the i&#39;th pair, first one is index &#39;0&#39;.
Definition: CSimpleMap.cpp:95
#define MRPT_START
#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...
static CGridPlaneXYPtr Create()
const GLdouble * v
Definition: glext.h:3603
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::maps::CMultiMetricMap CMultiMetricMap
Backward compatible typedef.
This class can be used to make partitions on a map/graph build from observations taken at some poses/...
int minimumNumberElementsEachCluster
If a partition leads to a cluster with less elements than this, it will be rejected even if had a goo...
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
void setListOfMaps(const mrpt::maps::TSetOfMetricMapInitializers *initializers)
Sets the list of internal map according to the passed list of map initializers (Current maps&#39; content...
static CSimpleLinePtr Create()
GLuint in
Definition: glext.h:6301
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:3908
#define ASSERT_(f)
float maxMahaDistForCorr
(Default: 2.0f) The minimum Mahalanobis distance between 2 probabilistic map elements for counting th...
bool insert_Landmarks_from_range_scans
If set to true (default), inserting a CObservation2DRangeScan in the map will generate landmarks for ...
ProxyFilterContainerByClass< mrpt::maps::CSimplePointsMapPtr, TListMaps > m_pointsMaps
STL-like proxy to access this kind of maps in maps.
This class stores any customizable set of metric maps.
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrix.h:30
void insert(const mrpt::poses::CPose3DPDF *in_posePDF, const mrpt::obs::CSensoryFrame &in_SF)
Add a new pair to the sequence.
Definition: CSimpleMap.cpp:201
void changeCoordinatesOrigin(const mrpt::poses::CPose3D &newOrigin)
Change the coordinate origin of all stored poses, for consistency with future new poses to enter in t...
Definition: CSimpleMap.cpp:318
GLfloat GLfloat p
Definition: glext.h:5587
Algorithms for finding the min-normalized-cut of a weighted undirected graph.
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:40
void clear()
Remove all stored pairs.
Definition: CSimpleMap.cpp:79
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
Definition: CStream.cpp:507
void remove(size_t index)
Deletes the i&#39;th pair, first one is index &#39;0&#39;.
Definition: CSimpleMap.cpp:110
std::vector< uint8_t > m_modified_nodes
The list of keyframes to consider in the next update.
static size_type size()
Definition: CPose3D.h:525



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019