MRPT  1.9.9
CHierarchicalMHMap.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-2019, 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 
13 #include <mrpt/poses/CPoint2D.h>
15 #include <cstring>
16 
17 using namespace mrpt::poses;
18 using namespace mrpt::slam;
19 using namespace mrpt::db;
20 using namespace mrpt::system;
21 using namespace mrpt::serialization;
22 using namespace mrpt::hmtslam;
23 using namespace std;
24 
26 
27 /*---------------------------------------------------------------
28  Constructor
29  ---------------------------------------------------------------*/
30 CHierarchicalMHMap::CHierarchicalMHMap() = default;
31 /*---------------------------------------------------------------
32  Destructor
33  ---------------------------------------------------------------*/
34 CHierarchicalMHMap::~CHierarchicalMHMap() { clear(); }
35 /*---------------------------------------------------------------
36  clear
37  ---------------------------------------------------------------*/
39 {
40  // Remaining arcs and nodes will be deleted.
41  // Using smart ptr makes this simple:
42  m_nodes.clear();
43  m_arcs.clear();
44 }
45 
46 uint8_t CHierarchicalMHMap::serializeGetVersion() const { return 0; }
47 void CHierarchicalMHMap::serializeTo(mrpt::serialization::CArchive& out) const
48 {
49  // Nodes:
50  out.WriteAs<uint32_t>(nodeCount());
51  for (const auto& n : m_nodes) out << *n.second;
52 
53  // Arcs:
54  out.WriteAs<uint32_t>(arcCount());
55  for (const auto& a : m_arcs) out << *a;
56 }
57 
58 void CHierarchicalMHMap::serializeFrom(
59  mrpt::serialization::CArchive& in, uint8_t version)
60 {
61  switch (version)
62  {
63  case 0:
64  {
65  uint32_t i, n;
66 
67  // Clear previous contents:
68  clear();
69 
70  // Nodes:
71  in >> n;
72  for (i = 0; i < n; i++)
73  {
74  CHMHMapNode::Ptr node = std::make_shared<CHMHMapNode>(
75  this); // This insert the node in my internal list via the
76  // callback method
77  in >> *node;
78  }
79 
80  // Arcs:
81  in >> n;
82  for (i = 0; i < n; i++)
83  {
84  // This insert the node in my internal list via the callback
85  // method
86  CHMHMapNode::Ptr p1, p2;
87  CHMHMapArc::Ptr arc = std::make_shared<CHMHMapArc>(
88  p1, p2, THypothesisIDSet(), this);
89  in >> *arc;
90  }
91  }
92  break;
93  default:
95  };
96 }
97 
98 /*---------------------------------------------------------------
99  onNodeDestruction
100  ---------------------------------------------------------------*/
101 void CHierarchicalMHMap::onNodeDestruction(CHMHMapNode* node)
102 {
103  TNodeList::iterator it;
104 
105  it = m_nodes.find(node->getID());
106 
107  if (it != m_nodes.end())
108  if (node == it->second.get()) m_nodes.erase(it);
109 }
110 
111 /*---------------------------------------------------------------
112  onArcDestruction
113  ---------------------------------------------------------------*/
114 void CHierarchicalMHMap::onArcDestruction(CHMHMapArc* arc)
115 {
116  // Important note: We cannot create a temporary smart pointer here, since
117  // it will lead to an infinity recursion! (BUGFIX, JLBC SEP-2009)
118  auto it = m_arcs.find_ptr_to(arc);
119  if (it != m_arcs.end()) m_arcs.erase(it);
120 }
121 
122 /*---------------------------------------------------------------
123  onNodeAddition
124  ---------------------------------------------------------------*/
125 void CHierarchicalMHMap::onNodeAddition(CHMHMapNode::Ptr& node)
126 {
127  // Check if it is not already in the list:
128  auto it = m_nodes.find(node->m_ID);
129 
130  if (it != m_nodes.end())
131  {
132  // Already in the list:
133  ASSERT_(node == it->second);
134  return;
135  }
136  else
137  {
138  // It is a new node: add to the list:
139  m_nodes[node->m_ID] = node;
140  }
141 }
142 
143 /*---------------------------------------------------------------
144  onArcAddition
145  ---------------------------------------------------------------*/
146 void CHierarchicalMHMap::onArcAddition(CHMHMapArc::Ptr& arc)
147 {
148  // Check if it is not already in the list:
149  auto it = m_arcs.find(arc);
150 
151  if (it == m_arcs.end()) // Is it new?
152  m_arcs.push_back(arc);
153 }
154 /*---------------------------------------------------------------
155  loadFromXMLfile
156  ---------------------------------------------------------------*/
157 
158 void CHierarchicalMHMap::loadFromXMLfile(std::string fileName)
159 {
160  CSimpleDatabase db;
162  size_t j, numnodes, numarcs;
163 
164  std::map<size_t, CHMHMapNode::Ptr> nodemap;
165  std::map<size_t, CHMHMapNode::Ptr>::iterator nodemapit;
166  using IDPair = std::pair<size_t, CHMHMapNode::Ptr>;
167 
168  std::map<size_t, CHMHMapNode::TNodeID> nodeanotmap;
169  std::map<size_t, CHMHMapNode::TNodeID>::iterator nodeanotmapit;
170  using IDnodeanotPair = std::pair<size_t, CHMHMapNode::TNodeID>;
171 
172  db.loadFromXML(fileName);
173 
174  table = db.getTable("nodes");
175  numnodes = table->getRecordCount();
176 
177  // printf("Loading nodes\n");
178  std::vector<std::string> node_anots;
179 
180  for (j = 0; j < numnodes; j++)
181  {
182  CHMHMapNode::Ptr node;
183  node = std::make_shared<CHMHMapNode>(this);
184  node->m_label = table->get(j, "nodename");
185  nodemap.insert(IDPair(atoi(table->get(j, "id").c_str()), node));
186  node->m_nodeType = table->get(j, "nodetype");
187  node->m_hypotheses.insert(COMMON_TOPOLOG_HYP);
188  printf("Loaded node %s\n", node->m_label.c_str());
189 
190  std::deque<std::string> lista;
191  mrpt::system::tokenize(table->get(j, "annotation-list"), " ", lista);
192 
193  for (auto& r : lista)
194  nodeanotmap.insert(
195  IDnodeanotPair((size_t)atoi(r.c_str()), node->getID()));
196 
197  // A map with key the id of annotations and value the id of nodes;
198  }
199 
200  table = db.getTable("arcs");
201  numarcs = table->getRecordCount();
202  printf("Loading arcs\n");
203  for (j = 0; j < numarcs; j++)
204  {
205  CHMHMapArc::Ptr arc, arcrev;
206  size_t from, to;
207  from = atoi(table->get(j, "from").c_str());
208  to = atoi(table->get(j, "to").c_str());
209 
210  CHMHMapNode::Ptr nodefrom, nodeto;
211  nodemapit = nodemap.find(from);
212  nodefrom = nodemapit->second;
213  std::cout << "finding nodes" << std::endl;
214 
215  nodemapit = nodemap.find(to);
216  nodeto = nodemapit->second;
217  std::cout << "added arc from " << nodefrom->m_label << " to "
218  << nodeto->m_label << std::endl;
219 
220  arc = std::make_shared<CHMHMapArc>(nodefrom, nodeto, 0, this);
221  arc->m_arcType = table->get(j, "arctype");
222  arc->m_hypotheses.insert(COMMON_TOPOLOG_HYP);
223 
224  if (atoi(table->get(j, "bidirectional").c_str()) == 1)
225  {
226  printf("Creating bidirectional arc\n");
227  arcrev = std::make_shared<CHMHMapArc>(nodeto, nodefrom, 0, this);
228  arcrev->m_arcType = table->get(j, "arctype");
229  arcrev->m_hypotheses.insert(COMMON_TOPOLOG_HYP);
230  }
231  }
232 
233  std::cout << "Graph with [" << numnodes << "] nodes and [" << numarcs
234  << "] arcs loaded successfully." << std::endl;
235 
236  table = db.getTable("annotations");
237  size_t numannot = table->getRecordCount();
238  printf("Loading annotations\n");
239  for (size_t ja = 0; ja < numannot; ja++)
240  {
241  string type = table->get(ja, "annotation-type");
242  string value = table->get(ja, "annotation-value");
243  nodeanotmapit = nodeanotmap.find(atoi(table->get(ja, "id").c_str()));
244 
245  if (nodeanotmapit != nodeanotmap.end())
246  {
247  if (type == "placePose")
248  {
249  CPoint2D::Ptr o = std::make_shared<CPoint2D>();
250  o->fromString(value);
251 
252  CHMHMapNode::Ptr node = getNodeByID(nodeanotmapit->second);
253 
254  node->m_annotations.set(
256  }
257  }
258  }
259 }
260 /*---------------------------------------------------------------
261  dumpAsXMLfile
262  ---------------------------------------------------------------*/
263 
264 void CHierarchicalMHMap::dumpAsXMLfile(std::string fileName) const
265 {
266  CSimpleDatabase db;
267  CSimpleDatabaseTable::Ptr tablenodes, tablearcs, tableannots;
268  size_t i;
269 
270  tablenodes = db.createTable("nodes");
271  tablearcs = db.createTable("arcs");
272  tableannots = db.createTable("annotations");
273 
274  tablenodes->addField("id");
275  tablenodes->addField("nodename");
276  tablenodes->addField("nodetype");
277  tablenodes->addField("annotation-list");
278 
279  tablearcs->addField("id");
280  tablearcs->addField("from");
281  tablearcs->addField("to");
282  tablearcs->addField("arctype");
283  tablearcs->addField("bidirectional");
284  tablearcs->addField("annotation-list");
285 
286  tableannots->addField("id");
287  tableannots->addField("annotation-type");
288  tableannots->addField("annotation-value");
289 
290  // for nodes
291  printf("Generating nodes\n");
292  for (const auto& m_node : m_nodes)
293  {
294  i = tablenodes->appendRecord();
295  tablenodes->set(i, "nodename", m_node.second->m_label.c_str());
296  tablenodes->set(
297  i, "id", format("%i", static_cast<int>(m_node.second->getID())));
298  tablenodes->set(i, "nodetype", m_node.second->m_nodeType);
299 
300  tablenodes->set(i, "annotation-list", ".");
301  for (auto ann = m_node.second->m_annotations.begin();
302  ann != m_node.second->m_annotations.end(); ++ann)
303  {
304  size_t j = tableannots->appendRecord();
305  tableannots->set(
306  j, "id", format("%u", static_cast<unsigned int>(j)));
307  tableannots->set(j, "annotation-type", ann->name.c_str());
308  ASSERT_(ann->value);
309  string str;
310  if (IS_CLASS(*ann->value, CPoint2D))
311  {
312  CPoint2D::Ptr o =
313  std::dynamic_pointer_cast<CPoint2D>(ann->value);
314  o->asString(str);
315  }
316  else
317  {
318  std::vector<uint8_t> v;
319  ObjectToOctetVector(ann->value.get(), v);
320  str.resize(v.size());
321  std::memcpy(&str[0], &v[0], v.size());
322  }
323  tableannots->set(j, "annotation-value", str);
324  if (tablenodes->get(j, "annotation-list") == ".")
325  tablenodes->set(
326  i, "annotation-list",
327  format("%u", static_cast<unsigned int>(j)));
328  else
329  tablenodes->set(
330  i, "annotation-list",
331  tablenodes->get(j, "annotation-list") +
332  format("%u", static_cast<unsigned int>(j)));
333  }
334  }
335 
336  // for arcs
337  printf("Generating arcs (%u)\n", static_cast<unsigned int>(m_arcs.size()));
338 
339  for (const auto& m_arc : m_arcs)
340  {
341  size_t fromid, toid;
342 
343  fromid = (int)m_arc->getNodeFrom();
344  toid = (int)m_arc->getNodeTo();
345 
346  i = tablearcs->appendRecord();
347  tablearcs->set(i, "id", format("%u", static_cast<unsigned int>(i)));
348  tablearcs->set(
349  i, "from", format("%u", static_cast<unsigned int>(fromid)));
350  tablearcs->set(i, "to", format("%u", static_cast<unsigned int>(toid)));
351  tablearcs->set(i, "arctype", m_arc->m_arcType);
352 
353  for (auto ann = m_arc->m_annotations.begin();
354  ann != m_arc->m_annotations.end(); ++ann)
355  {
356  i = tableannots->appendRecord();
357  tableannots->set(
358  i, "id", format("%u", static_cast<unsigned int>(i)));
359  tableannots->set(i, "annotation-type", ann->name.c_str());
360 
361  // CSerializable *o=ann->value->clone(); // JL: duplicate???
362  // tableannots->set(i,"annotation-value",ObjectToString(o));
363  }
364  }
365  printf("Generating XML file\n");
366  db.saveAsXML(fileName.c_str());
367 
368  /*
369  std::string s;
370  s += format("NODE ID: %i\t LABEL:%s\tARCS: ",
371  (int)it->second->getID(), it->second->m_label.c_str() );
372  TArcList arcs;
373  it->second->getArcs(arcs);
374  for (TArcList::const_iterator a=arcs.begin();a!=arcs.end();++a)
375  s += format("%i-%i, ", (int)(*a)->getNodeFrom(),
376  (int)(*a)->getNodeTo() );
377 
378  st << s;
379 
380  for (CMHPropertiesValuesList::const_iterator ann =
381  it->second->m_annotations.begin(); ann !=
382  it->second->m_annotations.end(); ++ann)
383  {
384  s= format(" [HYPO ID #%02i] Annotation '%s' Class: ",
385  (int)ann->ID, ann->name.c_str() );
386  if ( ann->value )
387  s+= string(ann->value->GetRuntimeClass()->className);
388  else s+= "(nullptr)";
389 
390  st << s;
391 
392  if ( ann->name == NODE_ANNOTATION_REF_POSEID )
393  {
394  TPoseID refID;
395  it->second->m_annotations.getElemental(NODE_ANNOTATION_REF_POSEID,
396  refID, ann->ID );
397  st << format(" VALUE: %i",(int)refID);
398  }
399  else if ( ann->name == NODE_ANNOTATION_POSES_GRAPH )
400  {
401  CRobotPosesGraph::Ptr posesGraph =
402  it->second->m_annotations.getAs<CRobotPosesGraph>(NODE_ANNOTATION_POSES_GRAPH,ann->ID);
403  ASSERT_(posesGraph);
404 
405  st << format(" CRobotPosesGraph has %i
406  poses:",(int)posesGraph->size());
407  CPose3D pdfMean;
408  for (CRobotPosesGraph::const_iterator
409  p=posesGraph->begin();p!=posesGraph->end();++p)
410  {
411  const CPose3DPDFParticles &pdf = p->second.pdf;
412  pdf.getMean(pdfMean);
413  st << format(" Pose %i \t (%.03f,%.03f,%.03fdeg)",
414  (int)p->first,
415  pdfMean.x(),
416  pdfMean.y(),
417  RAD2DEG(pdfMean.yaw()));
418  }
419  }
420  }
421 
422  st << "";
423 
424  }
425 
426 
427  st << "";
428  st << "";
429  st << "LIST OF ARCS";
430  st << "================";
431 
432  for (TArcList::const_iterator it=m_arcs.begin();it!=m_arcs.end();++it)
433  {
434  std::string s;
435  s += format("ARC: %i -> %i\n", (int)(*it)->getNodeFrom(),
436  (int)(*it)->getNodeTo() );
437 
438  s+= string(" Arc type: ")+(*it)->m_arcType;
439 
440  st << s;
441 
442  for (CMHPropertiesValuesList::const_iterator ann =
443  (*it)->m_annotations.begin(); ann != (*it)->m_annotations.end(); ++ann)
444  {
445  s= format(" [HYPO ID #%02i] Annotation '%s' Class: ",
446  (int)ann->ID, ann->name.c_str() );
447  if ( ann->value )
448  s+= string(ann->value->GetRuntimeClass()->className);
449  else s+= "(nullptr)";
450 
451  st << s;
452 
453  if ( ann->name == ARC_ANNOTATION_DELTA_SRC_POSEID )
454  {
455  TPoseID refID;
456  (*it)->m_annotations.getElemental(ARC_ANNOTATION_DELTA_SRC_POSEID,
457  refID, ann->ID );
458  st << format(" VALUE: %i",(int)refID);
459  }
460  else if ( ann->name == ARC_ANNOTATION_DELTA_TRG_POSEID )
461  {
462  TPoseID refID;
463  (*it)->m_annotations.getElemental(ARC_ANNOTATION_DELTA_TRG_POSEID,
464  refID, ann->ID );
465  st << format(" VALUE: %i",(int)refID);
466  }
467  else if ( ann->name == ARC_ANNOTATION_DELTA )
468  {
469  CSerializable::Ptr o =
470  (*it)->m_annotations.get(ARC_ANNOTATION_DELTA, ann->ID );
471  ASSERT_(o);
472 
473  CPose3DPDFGaussian relativePoseAcordToArc;
474  relativePoseAcordToArc.copyFrom(*CPose3DPDF::Ptr(o));
475 
476  st << format(" VALUE: (%f,%f,%f , %fdeg,%fdeg,%fdeg)",
477  relativePoseAcordToArc.mean.x(),
478  relativePoseAcordToArc.mean.y(),
479  relativePoseAcordToArc.mean.z(),
480  RAD2DEG( relativePoseAcordToArc.mean.yaw() ),
481  RAD2DEG( relativePoseAcordToArc.mean.pitch() ),
482  RAD2DEG( relativePoseAcordToArc.mean.roll() ) );
483  }
484  }
485 
486  st << "";
487  }
488  */
489 }
bool saveAsXML(const std::string &fileName) const
Saves this database as a XML file.
#define COMMON_TOPOLOG_HYP
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
This class impements a very simple database system.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
GLenum GLsizei n
Definition: glext.h:5136
GLenum GLsizei GLenum GLenum const GLvoid * table
Definition: glext.h:3535
void ObjectToOctetVector(const CSerializable *o, std::vector< uint8_t > &out_vector)
Converts (serializes) an MRPT object into an array of bytes.
STL namespace.
CSimpleDatabaseTable::Ptr createTable(const std::string &name)
Creates a new table in the DB, initially empty.
void tokenize(const std::string &inString, const std::string &inDelimiters, OUT_CONTAINER &outTokens, bool skipBlankTokens=true) noexcept
Tokenizes a string according to a set of delimiting characters.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
A set of hypothesis IDs, used for arcs and nodes in multi-hypothesis hybrid maps. ...
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class...
Definition: CObject.h:133
GLsizei const GLchar ** string
Definition: glext.h:4116
A class used to store a 2D point.
Definition: CPoint2D.h:32
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
CSimpleDatabaseTable::Ptr getTable(const std::string &tableName)
Returns the table with the indicated name.
const GLdouble * v
Definition: glext.h:3684
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
GLdouble GLdouble GLdouble r
Definition: glext.h:3711
mrpt::vision::TStereoCalibResults out
GLuint in
Definition: glext.h:7391
The most high level class for storing hybrid, multi-hypothesis maps in a graph-based model...
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:30
TNodeID getID() const
Reads the ID of the node (read-only property)
GLenum GLsizei GLenum format
Definition: glext.h:3535
A class for representing an arc between two nodes in a hierarchical, multi-hypothesis map...
Definition: CHMHMapArc.h:28
GLsizei const GLfloat * value
Definition: glext.h:4134
#define NODE_ANNOTATION_PLACE_POSE
GLubyte GLubyte GLubyte a
Definition: glext.h:6372
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:182
GLuint GLuint GLsizei GLenum type
Definition: glext.h:3532
A class for representing a node in a hierarchical, multi-hypothesis map.
Definition: CHMHMapNode.h:33
bool loadFromXML(const std::string &fileName)
Loads the content of this database from a a XML file.
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".
Definition: os.cpp:358



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: dad381fd7 Sun Oct 20 13:36:46 2019 +0200 at dom oct 20 13:40:10 CEST 2019