Main MRPT website > C++ reference for MRPT 1.5.7
CRobotPosesGraph.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 "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 
23 /*---------------------------------------------------------------
24  writeToStream
25  ---------------------------------------------------------------*/
26 void CRobotPosesGraph::writeToStream(mrpt::utils::CStream &out,int *version) const
27 {
28  if (version)
29  *version = 0;
30  else
31  {
32  uint32_t N = static_cast<uint32_t>(size());
33  out << N;
34 
36  {
37  out << it->first
38  << it->second.sf
39  << it->second.pdf;
40  }
41  }
42 }
43 
44 /*---------------------------------------------------------------
45  readFromStream
46  ---------------------------------------------------------------*/
47 void CRobotPosesGraph::readFromStream(mrpt::utils::CStream &in,int version)
48 {
49  switch(version)
50  {
51  case 0:
52  {
53  uint32_t i,N;
54  in >> N;
55  clear();
56 
57  for (i=0;i<N;i++)
58  {
59  TPoseID poseid;
60  in >> poseid;
61 
62  TPoseInfo &info = (*this)[poseid];
63 
64  in >> info.sf
65  >> info.pdf;
66  }
67 
68  } break;
69  default:
71 
72  };
73 }
74 
75 /*---------------------------------------------------------------
76  insertIntoMetricMap
77  ---------------------------------------------------------------*/
78 void CRobotPosesGraph::insertIntoMetricMap( CMultiMetricMap &metricMap ) const
79 {
80  CPose3D meanPose;
82  {
83  it->second.pdf.getMean(meanPose);
84  it->second.sf.insertObservationsInto( &metricMap, &meanPose );
85  }
86 }
87 
88 /*---------------------------------------------------------------
89  convertIntoSimplemap
90  ---------------------------------------------------------------*/
91 void CRobotPosesGraph::convertIntoSimplemap( CSimpleMap &out_simplemap) const
92 {
93  out_simplemap.clear();
95  out_simplemap.insert( &it->second.pdf, it->second.sf );
96 }
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
EIGEN_STRONG_INLINE iterator begin()
Definition: eigen_plugins.h:26
STL namespace.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:113
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
GLuint GLuint end
Definition: glext.h:3512
Auxiliary class used in mrpt::slam::CLocalMetricHypothesis for HMT-SLAM; this class stores a set of r...
int version
Definition: mrpt_jpeglib.h:898
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
backing_store_ptr info
Definition: jmemsys.h:170
GLuint in
Definition: glext.h:6301
Information kept for each robot pose used in CRobotPosesGraph.
typedef void(APIENTRYP PFNGLBLENDCOLORPROC)(GLclampf red
GLsizeiptr size
Definition: glext.h:3779
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:201
unsigned __int32 uint32_t
Definition: rptypes.h:49
void clear()
Remove all stored pairs.
Definition: CSimpleMap.cpp:79



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 8277875f6 Mon Jun 11 02:47:32 2018 +0200 at lun oct 28 01:50:49 CET 2019