Main MRPT website > C++ reference for MRPT 1.5.7
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-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 
11 #include "hmtslam-precomp.h" // Precomp header
12 
13 #include <mrpt/random.h>
14 
15 using namespace mrpt::slam;
16 using namespace mrpt::hmtslam;
17 using namespace std;
18 
20 
21 /*---------------------------------------------------------------
22  Default constructor
23  ---------------------------------------------------------------*/
25  CHierarchicalMHMap *parent,
26  const THypothesisIDSet &hyps) :
27  m_hypotheses(hyps),
28  m_ID(),
29  m_arcs(),
30  m_parent(parent),
31  m_nodeType( NODE_TYPES, DEFAULT_NODE_TYPE),
32  m_label("none")
33 {
34  // Assure that ID is unique in the graph:
35  // -----------------------------------------
36  if (m_parent.get())
37  {
38  // Parent will be NULL only in the default constructor for a temporary
39  // initialization before loading the object from "readFromStream"
40  m_ID=0;
41  do
42  {
43  m_ID++; /* = (((uint64_t)randomGenerator.drawUniform(0.0f,0xFFFF)) << 32) |
44  (((uint64_t)randomGenerator.drawUniform(0.0f,0xFFFF)) << 16) |
45  (((uint64_t)randomGenerator.drawUniform(0.0f,0xFFFF)));*/
46  } while (m_parent->getNodeByID(m_ID));
47  }
48 }
49 
50 CHMHMapNodePtr CHMHMapNode::Create(
51  CHierarchicalMHMap *parent,
52  const THypothesisIDSet &hyps )
53 {
54  CHMHMapNodePtr obj = CHMHMapNodePtr( new CHMHMapNode(parent,hyps) );
55  if (parent) parent->onNodeAddition(obj);
56  return obj;
57 }
58 
59 
60 /*---------------------------------------------------------------
61  Destructor
62  ---------------------------------------------------------------*/
63 CHMHMapNode::~CHMHMapNode()
64 {
65  // To the graph:
66  if (m_parent.get())
67  m_parent->onNodeDestruction(this);
68 
69  // To the arcs:
70  for (TArcList::iterator it=m_arcs.begin();it!=m_arcs.end();++it)
71  (*it)->onNodeDestruction(this);
72 }
73 
74 /*---------------------------------------------------------------
75  Implements the writing to a CStream capability of
76  CSerializable objects
77  ---------------------------------------------------------------*/
78 void CHMHMapNode::writeToStream(mrpt::utils::CStream &out,int *version) const
79 {
80  if (version)
81  *version = 0;
82  else
83  {
84  // Data:
85  out << m_ID << m_label;
86  out << m_nodeType.getType();
87  out << m_annotations;
88  out << m_hypotheses;
89  }
90 
91 }
92 
93 /*---------------------------------------------------------------
94  Implements the reading from a CStream capability of
95  CSerializable objects
96  ---------------------------------------------------------------*/
97 void CHMHMapNode::readFromStream(mrpt::utils::CStream &in,int version)
98 {
99  switch(version)
100  {
101  case 0:
102  {
104  // Data:
105  in >> m_ID >> m_label >> type >> m_annotations >> m_hypotheses;
106 
107  m_nodeType.setType(type);
108 
109  // It's not necessary since at ::Create this is already done (but...check!)
110  //if (m_parent.get())
111  // m_parent->onNodeAddition(this);
112 
113  } break;
114  default:
116 
117  };
118 
119 }
120 
121 /*---------------------------------------------------------------
122  onArcDestruction
123  ---------------------------------------------------------------*/
124 void CHMHMapNode::onArcDestruction(CHMHMapArc *arc)
125 {
126  MRPT_START
127 
128  // Important note: We cannot create a temporary smart pointer here, since
129  // it will lead to an infinity recursion! (BUGFIX, JLBC SEP-2009)
130 
131  // Check if arc is from/to this node:
132  if (arc->m_nodeFrom==m_ID || arc->m_nodeTo==m_ID)
133  {
134  // Remove from the list:
135  TArcList::iterator it = m_arcs.find_ptr_to(arc);
136  if (it!=m_arcs.end())
137  m_arcs.erase(it);
138  }
139 
140  MRPT_END
141 }
142 
143 /*---------------------------------------------------------------
144  onArcAddition
145  ---------------------------------------------------------------*/
146 void CHMHMapNode::onArcAddition(CHMHMapArcPtr &arc)
147 {
148  MRPT_START
149 
150  // Check if arc is from/to this node:
151  if (arc->m_nodeFrom==m_ID || arc->m_nodeTo==m_ID)
152  {
153  // Already in the list?
154  TArcList::iterator it = m_arcs.find(arc);
155  if (it==m_arcs.end())
156  m_arcs.push_back(arc); // Add to the list:
157  }
158 
159  MRPT_END
160 }
161 /*---------------------------------------------------------------
162  onArcAddition
163  ---------------------------------------------------------------*/
164 CHMHMapNode::TNodeID CHMHMapNode::getID() const
165 {
166  return m_ID;
167 }
168 
169 /*---------------------------------------------------------------
170  getLevelInTheHierarchy
171  ---------------------------------------------------------------*/
172 unsigned int CHMHMapNode::getLevelInTheHierarchy()
173 {
174  TArcList::iterator itArc;
175  unsigned int level = 0;
176 
177  for (itArc=m_arcs.begin();itArc!=m_arcs.end();itArc++)
178  {
179  // I am a "level+1" from the level below if a "belongs" arc points to me:
180  if ((*itArc)->m_arcType.isType("Membership") &&
181  (*itArc)->m_nodeTo == this->m_ID )
182  {
183  unsigned int L = m_parent->getNodeByID( (*itArc)->m_nodeFrom )->getLevelInTheHierarchy();
184  level = max(L+1, level);
185  }
186  }
187 
188  return level;
189 }
190 
191 /*---------------------------------------------------------------
192  getRelatedArcsCount
193  ---------------------------------------------------------------*/
194 unsigned int CHMHMapNode::getRelatedArcsCount()
195 {
196  return (unsigned int)m_arcs.size();
197 }
198 
199 /*---------------------------------------------------------------
200  getArcs
201  ---------------------------------------------------------------*/
202 void CHMHMapNode::getArcs( TArcList &out, const THypothesisID &hyp_id ) const
203 {
204  out.clear();
205  for (TArcList::const_iterator it=m_arcs.begin();it!=m_arcs.end();++it)
206  if ((*it)->m_hypotheses.has(hyp_id))
207  out.push_back(*it);
208 }
209 
210 
211 /*---------------------------------------------------------------
212  getArcs
213  ---------------------------------------------------------------*/
214 void CHMHMapNode::getArcs( TArcList &out, const char *arcType, const THypothesisID &hyp_id ) const
215 {
216  out.clear();
217  for (TArcList::const_iterator it=m_arcs.begin();it!=m_arcs.end();++it)
218  if ((*it)->m_hypotheses.has(hyp_id) && (*it)->m_arcType.isType(arcType) )
219  out.push_back(*it);
220 }
221 
222 /*---------------------------------------------------------------
223  isNeighbor
224  ---------------------------------------------------------------*/
225 bool CHMHMapNode::isNeighbor(const TNodeID &otherArea, const THypothesisID &hyp_id ) const
226 {
227  for (TArcList::const_iterator it=m_arcs.begin();it!=m_arcs.end();++it)
228  if ((*it)->m_hypotheses.has(hyp_id) &&
229  ( (*it)->m_nodeFrom==otherArea || (*it)->m_nodeTo==otherArea ) )
230  return true;
231  return false; // Nope
232 }
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.
Scalar * iterator
Definition: eigen_plugins.h:23
STL namespace.
CHMHMapNode::TNodeID m_nodeTo
Definition: CHMHMapArc.h:47
const Scalar * const_iterator
Definition: eigen_plugins.h:24
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
CHMHMapNode::TNodeID m_nodeFrom
The origin/target nodes for this arc.
Definition: CHMHMapArc.h:47
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
A set of hypothesis IDs, used for arcs and nodes in multi-hypothesis hybrid maps. ...
#define MRPT_END
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
int version
Definition: mrpt_jpeglib.h:898
GLsizei const GLchar ** string
Definition: glext.h:3919
#define MRPT_START
GLuint in
Definition: glext.h:6301
GLint level
Definition: glext.h:3545
The most high level class for storing hybrid, multi-hypothesis maps in a graph-based model...
#define DEFAULT_NODE_TYPE
class HMTSLAM_IMPEXP CHMHMapNode
#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
void onNodeAddition(CHMHMapNodePtr &node)
Event handler to be called just after a node has being created: it will be added to the internal list...
A class for storing a sequence of arcs (a path).
GLuint GLuint GLsizei GLenum type
Definition: glext.h:3512
A class for representing a node in a hierarchical, multi-hypothesis map.
Definition: CHMHMapNode.h:37
mrpt::utils::TNodeID TNodeID
The type of the IDs of nodes.
Definition: CHMHMapNode.h:49



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019