MRPT  1.9.9
CHMHMapArc.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 using namespace mrpt;
13 using namespace mrpt::slam;
14 using namespace mrpt::hmtslam;
15 
17 
18 /*---------------------------------------------------------------
19  Constructor
20  ---------------------------------------------------------------*/
22  const CHMHMapNode::TNodeID& from, const CHMHMapNode::TNodeID& to,
23  const THypothesisIDSet& hyps, CHierarchicalMHMap* parent)
24  : m_hypotheses(hyps),
25  m_nodeFrom(from),
26  m_nodeTo(to),
27  m_parent(parent),
28  m_arcType(ARC_TYPES, DEFAULT_ARC_TYPE),
29  m_annotations()
30 {
31  // parent will be nullptr only inside a ">>" operation, as a temporal
32  // initialization of an empty object with the default constructor:
33  // To the graph:
34 }
35 
36 /*---------------------------------------------------------------
37  Other constructor
38  ---------------------------------------------------------------*/
39 CHMHMapArc::CHMHMapArc(
40  CHMHMapNode::Ptr& from, CHMHMapNode::Ptr& to, const THypothesisIDSet& hyps,
41  CHierarchicalMHMap* parent)
42  : m_hypotheses(hyps),
43  m_nodeFrom(),
44  m_nodeTo(),
45  m_parent(parent),
46  m_arcType(ARC_TYPES, DEFAULT_ARC_TYPE),
47  m_annotations()
48 {
49  if (from) m_nodeFrom = from->getID();
50  if (to) m_nodeTo = to->getID();
51 
52  // parent will be nullptr only inside a ">>" operation, as a temporal
53  // initialization of an empty object with the default constructor:
54 }
55 
56 /*---------------------------------------------------------------
57  Destructor
58  ---------------------------------------------------------------*/
60 {
61  CHMHMapNode::Ptr node;
62  // To the nodes:
63  if ((node = m_parent->getNodeByID(m_nodeFrom)))
64  node->onArcDestruction(this);
65  if ((node = m_parent->getNodeByID(m_nodeTo))) node->onArcDestruction(this);
66 
67  // To the graph:
68  if (m_parent.get()) m_parent->onArcDestruction(this);
69 }
70 
71 /*---------------------------------------------------------------
72  onNodeDestruction
73  ---------------------------------------------------------------*/
75 {
77 
78  // Check if arc is from/to this node:
79  if (node->getID() == m_nodeFrom) m_nodeFrom = AREAID_INVALID;
80  if (node->getID() == m_nodeTo) m_nodeTo = AREAID_INVALID;
81 
82  MRPT_END
83 }
84 
85 uint8_t CHMHMapArc::serializeGetVersion() const { return 0; }
87 {
89 }
90 
92  mrpt::serialization::CArchive& in, uint8_t version)
93 {
94  switch (version)
95  {
96  case 0:
97  {
98  in >> m_nodeFrom >> m_nodeTo >> m_arcType >> m_annotations >>
100 
101  // Find my smart pointer in the HMT map: we MUST have only 1 smrt.
102  // pointer pointing to the same object!!
103  CHMHMapArc::Ptr myPtr;
104  for (auto it = m_parent->m_arcs.begin();
105  it != m_parent->m_arcs.end(); ++it)
106  {
107  if (it->get() == this)
108  {
109  myPtr = *it;
110  break;
111  }
112  }
113  ASSERTMSG_(myPtr, "I cannot be found in my parent HMT map!");
114 
115  CHMHMapNode::Ptr node;
116  // It's not necessary since at ::Create this is already done
117  // (but...check!)
118  // m_parent->onArcAddition(this);
119 
120  // To the nodes:
121  if ((node = m_parent->getNodeByID(m_nodeFrom)))
122  node->onArcAddition(myPtr);
123  if ((node = m_parent->getNodeByID(m_nodeTo)))
124  node->onArcAddition(myPtr);
125  }
126  break;
127  default:
129  };
130 }
131 
132 /*---------------------------------------------------------------
133  TArcList::debugDump
134  ---------------------------------------------------------------*/
136 {
137  printf("Dumping arcs list: %u elements\n", (unsigned int)size());
138  for (auto& i : *this)
139  {
140  printf(
141  "\t'%s'\t-> '%s'\n",
142  i->m_parent->getNodeByID(i->getNodeFrom())->m_label.c_str(),
143  i->m_parent->getNodeByID(i->getNodeTo())->m_label.c_str());
144  }
145 }
146 
148 {
149  uint32_t i, n;
150  in >> n;
151  BASE::clear();
152  for (i = 0; i < n; i++)
153  {
154  CHMHMapArc::Ptr theObj = std::make_shared<CHMHMapArc>();
155  in >> *theObj;
156  this->push_back(theObj);
157  }
158 }
160 {
161  out.WriteAs<uint32_t>(this->size());
162  for (const auto& i : *this) out << *i;
163 }
THypothesisIDSet m_hypotheses
The hypothesis IDs under which this arc exists.
Definition: CHMHMapArc.h:40
#define MRPT_START
Definition: exceptions.h:241
mrpt::safe_ptr< CHierarchicalMHMap > m_parent
The hierarchical graph in which this object is into.
Definition: CHMHMapArc.h:48
size_t size(const MATRIXLIKE &m, const int dim)
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.
#define ARC_TYPES
Used in constructor of mrpt::hmtslam::CHMHMapArc.
CHMHMapNode::TNodeID m_nodeTo
Definition: CHMHMapArc.h:45
#define AREAID_INVALID
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CHMHMapArc.cpp:85
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
void write(mrpt::serialization::CArchive &out) const
Definition: CHMHMapArc.cpp:159
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. ...
std::string m_arcType
The type of the arc, the possibilities are:
Definition: CHMHMapArc.h:98
CMHPropertiesValuesList m_annotations
The annotations of the arc, see the general description of the class for possible properties and valu...
Definition: CHMHMapArc.h:103
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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...
uint64_t TNodeID
A generic numeric type for unique IDs of nodes or entities.
Definition: TNodeID.h:16
TNodeID getID() const
Reads the ID of the node (read-only property)
void read(mrpt::serialization::CArchive &in)
Definition: CHMHMapArc.cpp:147
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CHMHMapArc.cpp:86
A class for representing an arc between two nodes in a hierarchical, multi-hypothesis map...
Definition: CHMHMapArc.h:28
~CHMHMapArc() override
Destructor.
Definition: CHMHMapArc.cpp:59
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CHMHMapArc.cpp:91
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
#define DEFAULT_ARC_TYPE
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:74



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