MRPT  1.9.9
CHMHMapNode.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-2018, 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 
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_ID(),
27  m_arcs(),
28  m_parent(parent),
29  m_nodeType(NODE_TYPES, DEFAULT_NODE_TYPE),
30  m_label("none")
31 {
32  // Assure that ID is unique in the graph:
33  // -----------------------------------------
34  if (m_parent.get())
35  {
36  // Parent will be nullptr only in the default constructor for a
37  // temporary
38  // initialization before loading the object from "readFromStream"
39  m_ID = 0;
40  do
41  {
42  m_ID++; /* =
43  (((uint64_t)getRandomGenerator().drawUniform(0.0f,0xFFFF))
44  << 32) |
45  (((uint64_t)getRandomGenerator().drawUniform(0.0f,0xFFFF)) <<
46  16) |
47  (((uint64_t)getRandomGenerator().drawUniform(0.0f,0xFFFF)));*/
48  } while (m_parent->getNodeByID(m_ID));
49  }
50 }
51 
52 /*---------------------------------------------------------------
53  Destructor
54  ---------------------------------------------------------------*/
55 CHMHMapNode::~CHMHMapNode()
56 {
57  // To the graph:
58  if (m_parent.get()) m_parent->onNodeDestruction(this);
59 
60  // To the arcs:
61  for (TArcList::iterator it = m_arcs.begin(); it != m_arcs.end(); ++it)
62  (*it)->onNodeDestruction(this);
63 }
64 
65 uint8_t CHMHMapNode::serializeGetVersion() const { return 0; }
66 void CHMHMapNode::serializeTo(mrpt::serialization::CArchive& out) const
67 {
68  out << m_ID << m_label;
69  out << m_nodeType;
70  out << m_annotations;
71  out << m_hypotheses;
72 }
73 
74 void CHMHMapNode::serializeFrom(
76 {
77  switch (version)
78  {
79  case 0:
80  {
81  in >> m_ID >> m_label >> m_nodeType >> m_annotations >>
82  m_hypotheses;
83 
84  // It's not necessary since at ::Create this is already done
85  // (but...check!)
86  // if (m_parent.get())
87  // m_parent->onNodeAddition(this);
88  }
89  break;
90  default:
92  };
93 }
94 
95 void CHMHMapNode::onArcDestruction(CHMHMapArc* arc)
96 {
98 
99  // Important note: We cannot create a temporary smart pointer here, since
100  // it will lead to an infinity recursion! (BUGFIX, JLBC SEP-2009)
101 
102  // Check if arc is from/to this node:
103  if (arc->m_nodeFrom == m_ID || arc->m_nodeTo == m_ID)
104  {
105  // Remove from the list:
106  TArcList::iterator it = m_arcs.find_ptr_to(arc);
107  if (it != m_arcs.end()) m_arcs.erase(it);
108  }
109 
110  MRPT_END
111 }
112 
113 /*---------------------------------------------------------------
114  onArcAddition
115  ---------------------------------------------------------------*/
116 void CHMHMapNode::onArcAddition(const CHMHMapArc::Ptr& arc)
117 {
118  MRPT_START
119 
120  // Check if arc is from/to this node:
121  if (arc->m_nodeFrom == m_ID || arc->m_nodeTo == m_ID)
122  {
123  // Already in the list?
124  TArcList::iterator it = m_arcs.find(arc);
125  if (it == m_arcs.end()) m_arcs.push_back(arc); // Add to the list:
126  }
127 
128  MRPT_END
129 }
130 /*---------------------------------------------------------------
131  onArcAddition
132  ---------------------------------------------------------------*/
133 CHMHMapNode::TNodeID CHMHMapNode::getID() const { return m_ID; }
134 /*---------------------------------------------------------------
135  getLevelInTheHierarchy
136  ---------------------------------------------------------------*/
137 unsigned int CHMHMapNode::getLevelInTheHierarchy()
138 {
139  TArcList::iterator itArc;
140  unsigned int level = 0;
141 
142  for (itArc = m_arcs.begin(); itArc != m_arcs.end(); itArc++)
143  {
144  // I am a "level+1" from the level below if a "belongs" arc points to
145  // me:
146  if ((*itArc)->m_arcType == "Membership" &&
147  (*itArc)->m_nodeTo == this->m_ID)
148  {
149  unsigned int L = m_parent->getNodeByID((*itArc)->m_nodeFrom)
150  ->getLevelInTheHierarchy();
151  level = max(L + 1, level);
152  }
153  }
154 
155  return level;
156 }
157 
158 /*---------------------------------------------------------------
159  getRelatedArcsCount
160  ---------------------------------------------------------------*/
161 unsigned int CHMHMapNode::getRelatedArcsCount()
162 {
163  return (unsigned int)m_arcs.size();
164 }
165 
166 /*---------------------------------------------------------------
167  getArcs
168  ---------------------------------------------------------------*/
169 void CHMHMapNode::getArcs(TArcList& out, const THypothesisID& hyp_id) const
170 {
171  out.clear();
172  for (TArcList::const_iterator it = m_arcs.begin(); it != m_arcs.end(); ++it)
173  if ((*it)->m_hypotheses.has(hyp_id)) out.push_back(*it);
174 }
175 
176 /*---------------------------------------------------------------
177  getArcs
178  ---------------------------------------------------------------*/
179 void CHMHMapNode::getArcs(
180  TArcList& out, const char* arcType, const THypothesisID& hyp_id) const
181 {
182  out.clear();
183  for (const auto& a : m_arcs)
184  if (a->m_hypotheses.has(hyp_id) && a->m_arcType == arcType)
185  out.push_back(a);
186 }
187 
188 /*---------------------------------------------------------------
189  isNeighbor
190  ---------------------------------------------------------------*/
191 bool CHMHMapNode::isNeighbor(
192  const TNodeID& otherArea, const THypothesisID& hyp_id) const
193 {
194  for (TArcList::const_iterator it = m_arcs.begin(); it != m_arcs.end(); ++it)
195  if ((*it)->m_hypotheses.has(hyp_id) &&
196  ((*it)->m_nodeFrom == otherArea || (*it)->m_nodeTo == otherArea))
197  return true;
198  return false; // Nope
199 }
Scalar * iterator
Definition: eigen_plugins.h:26
#define MRPT_START
Definition: exceptions.h:262
mrpt::graphs::TNodeID TNodeID
The type of the IDs of nodes.
Definition: CHMHMapNode.h:45
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.
STL namespace.
CHMHMapNode::TNodeID m_nodeTo
Definition: CHMHMapArc.h:46
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
CHMHMapNode::TNodeID m_nodeFrom
The origin/target nodes for this arc.
Definition: CHMHMapArc.h:46
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:52
#define MRPT_END
Definition: exceptions.h:266
GLuint in
Definition: glext.h:7274
GLint level
Definition: glext.h:3600
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:29
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
const Scalar * const_iterator
Definition: eigen_plugins.h:27
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:34



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020