MRPT  1.9.9
CHierarchicalMHMap.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 
12 #include <mrpt/poses/CPoint2D.h>
14 #include <cstring>
15 
16 using namespace mrpt::poses;
17 using namespace mrpt::slam;
18 using namespace mrpt::system;
19 using namespace mrpt::serialization;
20 using namespace mrpt::hmtslam;
21 using namespace std;
22 
24 
25 /*---------------------------------------------------------------
26  Constructor
27  ---------------------------------------------------------------*/
28 CHierarchicalMHMap::CHierarchicalMHMap() = default;
29 /*---------------------------------------------------------------
30  Destructor
31  ---------------------------------------------------------------*/
32 CHierarchicalMHMap::~CHierarchicalMHMap() { clear(); }
33 /*---------------------------------------------------------------
34  clear
35  ---------------------------------------------------------------*/
37 {
38  // Remaining arcs and nodes will be deleted.
39  // Using smart ptr makes this simple:
40  m_nodes.clear();
41  m_arcs.clear();
42 }
43 
44 uint8_t CHierarchicalMHMap::serializeGetVersion() const { return 0; }
45 void CHierarchicalMHMap::serializeTo(mrpt::serialization::CArchive& out) const
46 {
47  // Nodes:
48  out.WriteAs<uint32_t>(nodeCount());
49  for (const auto& n : m_nodes) out << *n.second;
50 
51  // Arcs:
52  out.WriteAs<uint32_t>(arcCount());
53  for (const auto& a : m_arcs) out << *a;
54 }
55 
56 void CHierarchicalMHMap::serializeFrom(
57  mrpt::serialization::CArchive& in, uint8_t version)
58 {
59  switch (version)
60  {
61  case 0:
62  {
63  uint32_t i, n;
64 
65  // Clear previous contents:
66  clear();
67 
68  // Nodes:
69  in >> n;
70  for (i = 0; i < n; i++)
71  {
72  CHMHMapNode::Ptr node = std::make_shared<CHMHMapNode>(
73  this); // This insert the node in my internal list via the
74  // callback method
75  in >> *node;
76  }
77 
78  // Arcs:
79  in >> n;
80  for (i = 0; i < n; i++)
81  {
82  // This insert the node in my internal list via the callback
83  // method
84  CHMHMapNode::Ptr p1, p2;
85  CHMHMapArc::Ptr arc = std::make_shared<CHMHMapArc>(
86  p1, p2, THypothesisIDSet(), this);
87  in >> *arc;
88  }
89  }
90  break;
91  default:
93  };
94 }
95 
96 /*---------------------------------------------------------------
97  onNodeDestruction
98  ---------------------------------------------------------------*/
99 void CHierarchicalMHMap::onNodeDestruction(CHMHMapNode* node)
100 {
101  TNodeList::iterator it;
102 
103  it = m_nodes.find(node->getID());
104 
105  if (it != m_nodes.end())
106  if (node == it->second.get()) m_nodes.erase(it);
107 }
108 
109 /*---------------------------------------------------------------
110  onArcDestruction
111  ---------------------------------------------------------------*/
112 void CHierarchicalMHMap::onArcDestruction(CHMHMapArc* arc)
113 {
114  // Important note: We cannot create a temporary smart pointer here, since
115  // it will lead to an infinity recursion! (BUGFIX, JLBC SEP-2009)
116  auto it = m_arcs.find_ptr_to(arc);
117  if (it != m_arcs.end()) m_arcs.erase(it);
118 }
119 
120 /*---------------------------------------------------------------
121  onNodeAddition
122  ---------------------------------------------------------------*/
123 void CHierarchicalMHMap::onNodeAddition(CHMHMapNode::Ptr& node)
124 {
125  // Check if it is not already in the list:
126  auto it = m_nodes.find(node->m_ID);
127 
128  if (it != m_nodes.end())
129  {
130  // Already in the list:
131  ASSERT_(node == it->second);
132  return;
133  }
134  else
135  {
136  // It is a new node: add to the list:
137  m_nodes[node->m_ID] = node;
138  }
139 }
140 
141 /*---------------------------------------------------------------
142  onArcAddition
143  ---------------------------------------------------------------*/
144 void CHierarchicalMHMap::onArcAddition(CHMHMapArc::Ptr& arc)
145 {
146  // Check if it is not already in the list:
147  auto it = m_arcs.find(arc);
148 
149  if (it == m_arcs.end()) // Is it new?
150  m_arcs.push_back(arc);
151 }
152 
153 void CHierarchicalMHMap::loadFromXMLfile(std::string fileName)
154 {
156  "Needs to be ported to tinyxml2! Please, give it a hand if you need "
157  "this method.");
158 }
159 
160 void CHierarchicalMHMap::dumpAsXMLfile(std::string fileName) const
161 {
163  "Needs to be ported to tinyxml2! Please, give it a hand if you need "
164  "this method.");
165 }
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
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.
STL namespace.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
A set of hypothesis IDs, used for arcs and nodes in multi-hypothesis hybrid maps. ...
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
mrpt::vision::TStereoCalibResults out
The most high level class for storing hybrid, multi-hypothesis maps in a graph-based model...
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:30
TNodeID getID() const
Reads the ID of the node (read-only property)
A class for representing an arc between two nodes in a hierarchical, multi-hypothesis map...
Definition: CHMHMapArc.h:28
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:183
A class for representing a node in a hierarchical, multi-hypothesis map.
Definition: CHMHMapNode.h:33



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