MRPT  2.0.4
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 }
CRobotPosesGraph.h
mrpt::containers::clear
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:183
mrpt::hmtslam::TPoseInfo
Information kept for each robot pose used in CRobotPosesGraph.
Definition: CRobotPosesGraph.h:22
out
mrpt::vision::TStereoCalibResults out
Definition: chessboard_stereo_camera_calib_unittest.cpp:25
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
hmtslam-precomp.h
mrpt::serialization::CArchive
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
mrpt::hmtslam::CRobotPosesGraph
Auxiliary class used in mrpt::slam::CLocalMetricHypothesis for HMT-SLAM; this class stores a set of r...
Definition: CRobotPosesGraph.h:36
mrpt::hmtslam
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
Definition: CHierarchicalMapMHPartition.h:27
mrpt::maps::CMultiMetricMap
This class stores any customizable set of metric maps.
Definition: CMultiMetricMap.h:120
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::math::size
size_t size(const MATRIXLIKE &m, const int dim)
Definition: math/include/mrpt/math/bits_math.h:21
IMPLEMENTS_SERIALIZABLE
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
Definition: CSerializable.h:166
mrpt::hmtslam::TPoseID
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
Definition: HMT_SLAM_common.h:63
mrpt::maps::CSimpleMap
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:32
mrpt::maps::CSimpleMap::insert
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
mrpt::maps
Definition: CBeacon.h:21
MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
mrpt::maps::CSimpleMap::clear
void clear()
Remove all stored pairs.
Definition: CSimpleMap.cpp:55
mrpt::slam
Definition: CMultiMetricMapPDF.h:26
mrpt::hmtslam::TPoseInfo::sf
mrpt::obs::CSensoryFrame sf
The observations.
Definition: CRobotPosesGraph.h:25
mrpt::hmtslam::TPoseInfo::pdf
mrpt::poses::CPose3DPDFParticles pdf
The robot pose PDF.
Definition: CRobotPosesGraph.h:27



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sun Jul 19 15:15:43 UTC 2020