MRPT  2.0.3
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-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 #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 
86  mrpt::config::CConfigFileBase& c, const std::string& s) const
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 
107  mrpt::config::CConfigFilePrefixer cfp(c, s + std::string("."), "");
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 }
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::config::CConfigFileBase::write
void write(const std::string &section, const std::string &name, enum_t value, const int name_padding_width=-1, const int value_padding_width=-1, const std::string &comment=std::string())
Definition: config/CConfigFileBase.h:107
MRPT_SAVE_CONFIG_VAR_COMMENT
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
Definition: config/CConfigFileBase.h:480
mrpt::opengl::CSetOfObjects::Ptr
std::shared_ptr< mrpt::opengl ::CSetOfObjects > Ptr
Definition: CSetOfObjects.h:28
mrpt::slam::CIncrementalMapPartitioner::serializeGetVersion
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CIncrementalMapPartitioner.cpp:461
MRPT_LOAD_CONFIG_VAR
#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...
Definition: config/CConfigFileBase.h:306
CSimpleLine.h
CSphere.h
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::serializeTo
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CIncrementalMapPartitioner.cpp:462
CSetOfObjects.h
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
MRPT_TODO
MRPT_TODO("toPointCloud / calibration")
mrpt::obs::CSensoryFrame::insertObservationsInto
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).
Definition: CSensoryFrame.cpp:266
eval_similarity_observation_overlap
static double eval_similarity_observation_overlap(const map_keyframe_t &kf1, const map_keyframe_t &kf2, const mrpt::poses::CPose3D &relPose2wrt1)
Definition: CIncrementalMapPartitioner.cpp:45
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::poses::CPose3DPDF
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually).
Definition: CPose3DPDF.h:39
mrpt::maps::CSimplePointsMap::TMapDefinition
Definition: CSimplePointsMap.h:135
mrpt::slam::CIncrementalMapPartitioner::clear
void clear()
Definition: CIncrementalMapPartitioner.cpp:111
stl_serialization.h
out
mrpt::vision::TStereoCalibResults out
Definition: chessboard_stereo_camera_calib_unittest.cpp:25
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
observations_overlap.h
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
mrpt::graphs::CGraphPartitioner
Finds the min-normalized-cut of a weighted undirected graph.
Definition: CGraphPartitioner.h:39
mrpt::slam::smMETRIC_MAP_MATCHING
@ smMETRIC_MAP_MATCHING
Definition: CIncrementalMapPartitioner.h:29
mrpt::serialization::CArchive
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
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_LOAD_HERE_CONFIG_VAR
#define MRPT_LOAD_HERE_CONFIG_VAR( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
Definition: config/CConfigFileBase.h:352
CConfigFilePrefixer.h
mrpt::opengl::CGridPlaneXY::Create
static Ptr Create(Args &&... args)
Definition: CGridPlaneXY.h:31
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
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::config::CConfigFilePrefixer
A wrapper for other CConfigFileBase-based objects that prefixes a given token to every key and/or sec...
Definition: CConfigFilePrefixer.h:37
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
mrpt::poses::CPoseOrPoint::y
double y() const
Definition: CPoseOrPoint.h:147
mrpt::graphs
Abstract graph and tree data structures, plus generic graph algorithms.
Definition: CAStarAlgorithm.h:15
mrpt::slam::map_keyframe_t
Map keyframe, comprising raw observations and they as a metric map.
Definition: CIncrementalMapPartitioner.h:37
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::keep_max
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.
Definition: core/include/mrpt/core/bits_math.h:152
mrpt::keep_min
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.
Definition: core/include/mrpt/core/bits_math.h:145
eval_similarity_metric_map_matching
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)
Definition: CIncrementalMapPartitioner.cpp:38
IMPLEMENTS_SERIALIZABLE
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
Definition: CSerializable.h:166
mrpt::slam::map_keyframe_t::kf_id
uint32_t kf_id
Definition: CIncrementalMapPartitioner.h:39
mrpt::opengl::CSphere::Ptr
std::shared_ptr< mrpt::opengl ::CSphere > Ptr
Definition: CSphere.h:31
mrpt::slam::CIncrementalMapPartitioner::changeCoordinatesOrigin
void changeCoordinatesOrigin(const mrpt::poses::CPose3D &newOrigin)
Definition: CIncrementalMapPartitioner.cpp:339
mrpt::config::MRPT_SAVE_VALUE_PADDING
int MRPT_SAVE_VALUE_PADDING()
Definition: CConfigFileBase.cpp:32
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::math::CMatrixDynamic::resize
void resize(size_t row, size_t col)
Definition: CMatrixDynamic.h:356
mrpt::slam::CIncrementalMapPartitioner
Finds partitions in metric maps based on N-cut graph partition theory.
Definition: CIncrementalMapPartitioner.h:54
mrpt::slam::CIncrementalMapPartitioner::serializeFrom
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CIncrementalMapPartitioner.cpp:438
CTicTac.h
slam-precomp.h
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_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::slam::CIncrementalMapPartitioner::TOptions::TOptions
TOptions()
Definition: CIncrementalMapPartitioner.cpp:53
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::poses::CPose3DPDF::Ptr
std::shared_ptr< CPose3DPDF > Ptr
Definition: CPose3DPDF.h:42
CIncrementalMapPartitioner.h
mrpt::maps
Definition: CBeacon.h:21
mrpt::slam::observationsOverlap
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...
Definition: observations_overlap.cpp:26
MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
CGridPlaneXY.h
mrpt::slam
Definition: CMultiMetricMapPDF.h:26
CGraphPartitioner.h
CPosePDFParticles.h
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::config::MRPT_SAVE_NAME_PADDING
int MRPT_SAVE_NAME_PADDING()
Default padding sizes for macros MRPT_SAVE_CONFIG_VAR_COMMENT(), etc.
Definition: CConfigFileBase.cpp:31
mrpt::format
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
CPose3DPDFParticles.h
mrpt::opengl::CSimpleLine::Ptr
std::shared_ptr< mrpt::opengl ::CSimpleLine > Ptr
Definition: CSimpleLine.h:21
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.3 at Fri May 29 13:06:46 UTC 2020