Main MRPT website > C++ reference for MRPT 1.5.7
CHMHMapArc.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 
12 using namespace mrpt;
13 using namespace mrpt::slam;
14 using namespace mrpt::hmtslam;
15 
17 
18 /*---------------------------------------------------------------
19  Constructor
20  ---------------------------------------------------------------*/
22  const CHMHMapNode::TNodeID &from ,
23  const CHMHMapNode::TNodeID &to,
24  const THypothesisIDSet &hyps,
25  CHierarchicalMHMap *parent) :
26  m_hypotheses(hyps),
27  m_nodeFrom(from),
28  m_nodeTo(to),
29  m_parent(parent),
30  m_arcType(ARC_TYPES,DEFAULT_ARC_TYPE ),
31  m_annotations()
32 {
33  // parent will be NULL only inside a ">>" operation, as a temporal
34  // initialization of an empty object with the default constructor:
35  // To the graph:
36 }
37 
38 CHMHMapArcPtr CHMHMapArc::Create(
39  const CHMHMapNode::TNodeID &from,
40  const CHMHMapNode::TNodeID &to,
41  const THypothesisIDSet &hyps,
42  CHierarchicalMHMap *parent)
43 {
44  CHMHMapArcPtr obj = CHMHMapArcPtr(new CHMHMapArc(from,to,hyps,parent));
45  if (parent)
46  {
47  parent->onArcAddition(obj);
48  CHMHMapNodePtr node;
49  if ( (node = parent->getNodeByID(from)).present() )
50  node->onArcAddition(obj);
51  if ( (node = parent->getNodeByID(to)).present() )
52  node->onArcAddition(obj);
53  }
54  return obj;
55 }
56 
57 
58 /*---------------------------------------------------------------
59  Other constructor
60  ---------------------------------------------------------------*/
62  CHMHMapNodePtr &from,
63  CHMHMapNodePtr &to,
64  const THypothesisIDSet &hyps,
65  CHierarchicalMHMap *parent) :
66  m_hypotheses(hyps),
67  m_nodeFrom(),
68  m_nodeTo(),
69  m_parent(parent),
70  m_arcType(ARC_TYPES,DEFAULT_ARC_TYPE ),
71  m_annotations()
72 {
73  if (from) m_nodeFrom = from->getID();
74  if (to) m_nodeTo = to->getID();
75 
76  // parent will be NULL only inside a ">>" operation, as a temporal
77  // initialization of an empty object with the default constructor:
78 }
79 
80 CHMHMapArcPtr CHMHMapArc::Create(
81  CHMHMapNodePtr &from,
82  CHMHMapNodePtr &to,
83  const THypothesisIDSet &hyps,
84  CHierarchicalMHMap *parent)
85 {
86  CHMHMapArcPtr obj = CHMHMapArcPtr(new CHMHMapArc(from,to,hyps,parent));
87  if (parent)
88  {
89  // To the graph:
90  parent->onArcAddition(obj);
91  // To the nodes:
92  if (from) from->onArcAddition(obj);
93  if (to) to->onArcAddition(obj);
94  }
95  return obj;
96 }
97 
98 
99 /*---------------------------------------------------------------
100  Destructor
101  ---------------------------------------------------------------*/
103 {
104  CHMHMapNodePtr node;
105  // To the nodes:
106  if ((node = m_parent->getNodeByID(m_nodeFrom)).present())
107  node->onArcDestruction(this);
108  if ((node = m_parent->getNodeByID(m_nodeTo)).present())
109  node->onArcDestruction(this);
110 
111  // To the graph:
112  if (m_parent.get())
113  m_parent->onArcDestruction(this);
114 }
115 
116 /*---------------------------------------------------------------
117  onNodeDestruction
118  ---------------------------------------------------------------*/
120 {
121  MRPT_START
122 
123  // Check if arc is from/to this node:
124  if (node->getID()==m_nodeFrom) m_nodeFrom = AREAID_INVALID;
125  if (node->getID()==m_nodeTo) m_nodeTo = AREAID_INVALID;
126 
127  MRPT_END
128 }
129 
130 
131 /*---------------------------------------------------------------
132  writeToStream
133  ---------------------------------------------------------------*/
135 {
136  if (version)
137  *version = 0;
138  else
139  {
141  }
142 
143 }
144 
145 /*---------------------------------------------------------------
146  readFromStream
147  ---------------------------------------------------------------*/
149 {
150  switch(version)
151  {
152  case 0:
153  {
155 
157 
159 
160  // Find my smart pointer in the HMT map: we MUST have only 1 smrt. pointer pointing to the same object!!
161  CHMHMapArcPtr myPtr;
162  for (TArcList::const_iterator it = m_parent->m_arcs.begin();it != m_parent->m_arcs.end();++it)
163  {
164  if (it->pointer()==this)
165  {
166  myPtr = *it;
167  break;
168  }
169  }
170  ASSERTMSG_(myPtr.present(),"I cannot be found in my parent HMT map!")
171 
172  CHMHMapNodePtr node;
173  // It's not necessary since at ::Create this is already done (but...check!)
174  //m_parent->onArcAddition(this);
175 
176  // To the nodes:
177  if ((node = m_parent->getNodeByID(m_nodeFrom)).present())
178  node->onArcAddition(myPtr);
179  if ((node = m_parent->getNodeByID(m_nodeTo)).present())
180  node->onArcAddition(myPtr);
181 
182  } break;
183  default:
185 
186  };
187 
188 }
189 
190 /*---------------------------------------------------------------
191  TArcList::debugDump
192  ---------------------------------------------------------------*/
194 {
195  printf("Dumping arcs list: %u elements\n", (unsigned int)size());
196  for (iterator i=begin();i!=end();++i)
197  {
198  printf("\t'%s'\t-> '%s'\n", (*i)->m_parent->getNodeByID( (*i)->getNodeFrom() )->m_label.c_str(),
199  (*i)->m_parent->getNodeByID( (*i)->getNodeTo() )->m_label.c_str() );
200  }
201 }
202 
203 
205 {
206  uint32_t i,n;
207  in >> n;
208  BASE::clear();
209  for (i=0;i<n;i++)
210  {
211  CHMHMapArcPtr theObj = CHMHMapArc::Create();
212  in >> *theObj;
213  this->push_back( theObj );
214  }
215 }
216 void TArcList::write( utils::CStream &out ) const
217 {
218  out << static_cast<uint32_t>(this->size());
219  for (const_iterator i=begin();i!=end();++i)
220  out << **i;
221 }
222 
THypothesisIDSet m_hypotheses
The hypothesis IDs under which this arc exists.
Definition: CHMHMapArc.h:42
void setType(const std::string &type)
Sets the currently selected type.
static CHMHMapArcPtr Create()
void read(utils::CStream &in)
Definition: CHMHMapArc.cpp:204
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.
utils::CMHPropertiesValuesList m_annotations
The annotations of the arc, see the general description of the class for possible properties and valu...
Definition: CHMHMapArc.h:113
class HMTSLAM_IMPEXP CHMHMapArc
Definition: CHMHMapNode.h:26
void readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
Definition: CHMHMapArc.cpp:148
GLenum GLsizei n
Definition: glext.h:4618
Scalar * iterator
Definition: eigen_plugins.h:23
EIGEN_STRONG_INLINE iterator begin()
Definition: eigen_plugins.h:26
EIGEN_STRONG_INLINE void push_back(Scalar val)
Insert an element at the end of the container (for 1D vectors/arrays)
CHMHMapNodePtr getNodeByID(CHMHMapNode::TNodeID id)
Returns the node with the given ID, or NULL if it does not exist.
#define ARC_TYPES
Used in constructor of mrpt::hmtslam::CHMHMapArc.
CHMHMapNode::TNodeID m_nodeTo
Definition: CHMHMapArc.h:47
const Scalar * const_iterator
Definition: eigen_plugins.h:24
#define AREAID_INVALID
utils::CTypeSelector m_arcType
The type of the arc, the possibilities are:
Definition: CHMHMapArc.h:109
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:113
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
Definition: CHMHMapArc.cpp:134
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.
GLuint GLuint end
Definition: glext.h:3512
uint64_t TNodeID
The type for node IDs in graphs of different types.
Definition: types_simple.h:45
int version
Definition: mrpt_jpeglib.h:898
virtual ~CHMHMapArc()
Destructor.
Definition: CHMHMapArc.cpp:102
void onArcAddition(CHMHMapArcPtr &arc)
Event handler to be called just after an arc has being created: it will be added to the internal list...
GLsizei const GLchar ** string
Definition: glext.h:3919
std::string getType() const
Gets the currently selected type, from the set of posibilities.
#define MRPT_START
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
GLuint in
Definition: glext.h:6301
The most high level class for storing hybrid, multi-hypothesis maps in a graph-based model...
CHMHMapArc(const CHMHMapNode::TNodeID &from=0, const CHMHMapNode::TNodeID &to=0, const THypothesisIDSet &hyps=THypothesisIDSet(), CHierarchicalMHMap *parent=NULL)
Private constructor (see ::Create class factory)
Definition: CHMHMapArc.cpp:21
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:29
GLsizeiptr size
Definition: glext.h:3779
unsigned __int32 uint32_t
Definition: rptypes.h:49
#define ASSERTMSG_(f, __ERROR_MSG)
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
#define DEFAULT_ARC_TYPE
mrpt::utils::safe_ptr< CHierarchicalMHMap > m_parent
The hierarchical graph in which this object is into.
Definition: CHMHMapArc.h:50
mrpt::utils::TNodeID TNodeID
The type of the IDs of nodes.
Definition: CHMHMapNode.h:49
void onNodeDestruction(CHMHMapNode *node)
Event handler to be called just before a node is being destroyed: it should be called only for nodes ...
Definition: CHMHMapArc.cpp:119
void write(utils::CStream &out) const
Definition: CHMHMapArc.cpp:216



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