MRPT  1.9.9
CRobotPosesGraph.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 "hmtslam-precomp.h" // Precomp header
11 
13 
14 using namespace mrpt::slam;
15 using namespace mrpt::maps;
16 using namespace mrpt::poses;
17 using namespace mrpt::hmtslam;
18 using namespace std;
19 
21 
22 uint8_t CRobotPosesGraph::serializeGetVersion() const { return 0; }
23 void CRobotPosesGraph::serializeTo(mrpt::serialization::CArchive& out) const
24 {
25  auto N = static_cast<uint32_t>(size());
26  out << N;
27  for (const auto& e : *this) out << e.first << e.second.sf << e.second.pdf;
28 }
29 
30 void CRobotPosesGraph::serializeFrom(
31  mrpt::serialization::CArchive& in, uint8_t version)
32 {
33  switch (version)
34  {
35  case 0:
36  {
37  uint32_t i, N;
38  in >> N;
39  clear();
40 
41  for (i = 0; i < N; i++)
42  {
43  TPoseID poseid;
44  in >> poseid;
45 
46  TPoseInfo& info = (*this)[poseid];
47 
48  in >> info.sf >> info.pdf;
49  }
50  }
51  break;
52  default:
54  };
55 }
56 
57 /*---------------------------------------------------------------
58  insertIntoMetricMap
59  ---------------------------------------------------------------*/
60 void CRobotPosesGraph::insertIntoMetricMap(CMultiMetricMap& metricMap) const
61 {
62  CPose3D meanPose;
63  for (const auto& it : *this)
64  {
65  it.second.pdf.getMean(meanPose);
66  it.second.sf.insertObservationsInto(&metricMap, &meanPose);
67  }
68 }
69 
70 /*---------------------------------------------------------------
71  convertIntoSimplemap
72  ---------------------------------------------------------------*/
73 void CRobotPosesGraph::convertIntoSimplemap(CSimpleMap& out_simplemap) const
74 {
75  out_simplemap.clear();
76  for (const auto& it : *this)
77  out_simplemap.insert(&it.second.pdf, it.second.sf);
78 }
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:32
size_t size(const MATRIXLIKE &m, const int dim)
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
mrpt::poses::CPose3DPDFParticles pdf
The robot pose PDF.
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
STL namespace.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
Auxiliary class used in mrpt::slam::CLocalMetricHypothesis for HMT-SLAM; this class stores a set of r...
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
mrpt::obs::CSensoryFrame sf
The observations.
Information kept for each robot pose used in CRobotPosesGraph.
This class stores any customizable set of metric maps.
void insert(const mrpt::poses::CPose3DPDF *in_posePDF, const mrpt::obs::CSensoryFrame &in_SF)
Add a new pair to the sequence.
Definition: CSimpleMap.cpp:138
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:183
void clear()
Remove all stored pairs.
Definition: CSimpleMap.cpp:55



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020