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