MRPT  2.0.1
CHMHMapNode.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/random.h>
13 
14 using namespace mrpt::slam;
15 using namespace mrpt::hmtslam;
16 using namespace std;
17 
19 
20 /*---------------------------------------------------------------
21  Default constructor
22  ---------------------------------------------------------------*/
24  CHierarchicalMHMap* parent, const THypothesisIDSet& hyps)
25  : m_hypotheses(hyps),
26  m_arcs(),
27  m_parent(parent),
28  m_nodeType(NODE_TYPES, DEFAULT_NODE_TYPE),
29  m_label("none")
30 {
31  // Assure that ID is unique in the graph:
32  // -----------------------------------------
33  if (m_parent.get())
34  {
35  // Parent will be nullptr only in the default constructor for a
36  // temporary
37  // initialization before loading the object from "readFromStream"
38  m_ID = 0;
39  do
40  {
41  m_ID++; /* =
42  (((uint64_t)getRandomGenerator().drawUniform(0.0f,0xFFFF))
43  << 32) |
44  (((uint64_t)getRandomGenerator().drawUniform(0.0f,0xFFFF)) <<
45  16) |
46  (((uint64_t)getRandomGenerator().drawUniform(0.0f,0xFFFF)));*/
47  } while (m_parent->getNodeByID(m_ID));
48  }
49 }
50 
51 /*---------------------------------------------------------------
52  Destructor
53  ---------------------------------------------------------------*/
54 CHMHMapNode::~CHMHMapNode()
55 {
56  // To the graph:
57  if (m_parent.get()) m_parent->onNodeDestruction(this);
58 
59  // To the arcs:
60  for (auto& m_arc : m_arcs) m_arc->onNodeDestruction(this);
61 }
62 
63 uint8_t CHMHMapNode::serializeGetVersion() const { return 0; }
64 void CHMHMapNode::serializeTo(mrpt::serialization::CArchive& out) const
65 {
66  out << m_ID << m_label;
67  out << m_nodeType;
68  out << m_annotations;
69  out << m_hypotheses;
70 }
71 
72 void CHMHMapNode::serializeFrom(
73  mrpt::serialization::CArchive& in, uint8_t version)
74 {
75  switch (version)
76  {
77  case 0:
78  {
79  in >> m_ID >> m_label >> m_nodeType >> m_annotations >>
80  m_hypotheses;
81 
82  // It's not necessary since at ::Create this is already done
83  // (but...check!)
84  // if (m_parent.get())
85  // m_parent->onNodeAddition(this);
86  }
87  break;
88  default:
90  };
91 }
92 
93 void CHMHMapNode::onArcDestruction(CHMHMapArc* arc)
94 {
96 
97  // Important note: We cannot create a temporary smart pointer here, since
98  // it will lead to an infinity recursion! (BUGFIX, JLBC SEP-2009)
99 
100  // Check if arc is from/to this node:
101  if (arc->m_nodeFrom == m_ID || arc->m_nodeTo == m_ID)
102  {
103  // Remove from the list:
104  auto it = m_arcs.find_ptr_to(arc);
105  if (it != m_arcs.end()) m_arcs.erase(it);
106  }
107 
108  MRPT_END
109 }
110 
111 /*---------------------------------------------------------------
112  onArcAddition
113  ---------------------------------------------------------------*/
114 void CHMHMapNode::onArcAddition(const CHMHMapArc::Ptr& arc)
115 {
116  MRPT_START
117 
118  // Check if arc is from/to this node:
119  if (arc->m_nodeFrom == m_ID || arc->m_nodeTo == m_ID)
120  {
121  // Already in the list?
122  auto it = m_arcs.find(arc);
123  if (it == m_arcs.end()) m_arcs.push_back(arc); // Add to the list:
124  }
125 
126  MRPT_END
127 }
128 /*---------------------------------------------------------------
129  onArcAddition
130  ---------------------------------------------------------------*/
131 CHMHMapNode::TNodeID CHMHMapNode::getID() const { return m_ID; }
132 /*---------------------------------------------------------------
133  getLevelInTheHierarchy
134  ---------------------------------------------------------------*/
135 unsigned int CHMHMapNode::getLevelInTheHierarchy()
136 {
137  TArcList::iterator itArc;
138  unsigned int level = 0;
139 
140  for (itArc = m_arcs.begin(); itArc != m_arcs.end(); itArc++)
141  {
142  // I am a "level+1" from the level below if a "belongs" arc points to
143  // me:
144  if ((*itArc)->m_arcType == "Membership" &&
145  (*itArc)->m_nodeTo == this->m_ID)
146  {
147  unsigned int L = m_parent->getNodeByID((*itArc)->m_nodeFrom)
148  ->getLevelInTheHierarchy();
149  level = max(L + 1, level);
150  }
151  }
152 
153  return level;
154 }
155 
156 /*---------------------------------------------------------------
157  getRelatedArcsCount
158  ---------------------------------------------------------------*/
159 unsigned int CHMHMapNode::getRelatedArcsCount()
160 {
161  return (unsigned int)m_arcs.size();
162 }
163 
164 /*---------------------------------------------------------------
165  getArcs
166  ---------------------------------------------------------------*/
167 void CHMHMapNode::getArcs(TArcList& out, const THypothesisID& hyp_id) const
168 {
169  out.clear();
170  for (const auto& m_arc : m_arcs)
171  if (m_arc->m_hypotheses.has(hyp_id)) out.push_back(m_arc);
172 }
173 
174 /*---------------------------------------------------------------
175  getArcs
176  ---------------------------------------------------------------*/
177 void CHMHMapNode::getArcs(
178  TArcList& out, const char* arcType, const THypothesisID& hyp_id) const
179 {
180  out.clear();
181  for (const auto& a : m_arcs)
182  if (a->m_hypotheses.has(hyp_id) && a->m_arcType == arcType)
183  out.push_back(a);
184 }
185 
186 /*---------------------------------------------------------------
187  isNeighbor
188  ---------------------------------------------------------------*/
189 bool CHMHMapNode::isNeighbor(
190  const TNodeID& otherArea, const THypothesisID& hyp_id) const
191 {
192  for (const auto& m_arc : m_arcs)
193  if (m_arc->m_hypotheses.has(hyp_id) &&
194  (m_arc->m_nodeFrom == otherArea || m_arc->m_nodeTo == otherArea))
195  return true;
196  return false; // Nope
197 }
#define MRPT_START
Definition: exceptions.h:241
mrpt::graphs::TNodeID TNodeID
The type of the IDs of nodes.
Definition: CHMHMapNode.h:44
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.
CHMHMapNode::TNodeID m_nodeTo
Definition: CHMHMapArc.h:45
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
CHMHMapNode::TNodeID m_nodeFrom
The origin/target nodes for this arc.
Definition: CHMHMapArc.h:45
A set of hypothesis IDs, used for arcs and nodes in multi-hypothesis hybrid maps. ...
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
mrpt::vision::TStereoCalibResults out
#define MRPT_END
Definition: exceptions.h:245
The most high level class for storing hybrid, multi-hypothesis maps in a graph-based model...
#define DEFAULT_NODE_TYPE
#define NODE_TYPES
Used in constructor of mrpt::hmtslam::CHMHMapNode.
A class for representing an arc between two nodes in a hierarchical, multi-hypothesis map...
Definition: CHMHMapArc.h:28
A class for storing a sequence of arcs (a path).
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 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020