MRPT  1.9.9
CHierarchicalMHMap.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-2018, 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 
14 #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  ---------------------------------------------------------------*/
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 = mrpt::make_aligned_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 = mrpt::make_aligned_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 {
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  TArcList::iterator 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  TNodeList::iterator 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  TArcList::iterator 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;
165  using IDPair = std::pair<size_t, CHMHMapNode::Ptr>;
166 
167  std::map<size_t, CHMHMapNode::TNodeID> nodeanotmap;
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 = mrpt::make_aligned_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 (size_t r = 0; r < lista.size(); r++)
193  nodeanotmap.insert(
194  IDnodeanotPair((size_t)atoi(lista[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 = mrpt::make_aligned_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 = mrpt::make_aligned_shared<CHMHMapArc>(
227  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 j = 0; j < numannot; j++)
240  {
241  string type = table->get(j, "annotation-type");
242  string value = table->get(j, "annotation-value");
243  nodeanotmapit = nodeanotmap.find(atoi(table->get(j, "id").c_str()));
244 
245  if (nodeanotmapit != nodeanotmap.end())
246  {
247  if (type == "placePose")
248  {
249  CPoint2D::Ptr o = mrpt::make_aligned_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 (TNodeList::const_iterator it = m_nodes.begin(); it != m_nodes.end();
293  ++it)
294  {
295  i = tablenodes->appendRecord();
296  tablenodes->set(i, "nodename", it->second->m_label.c_str());
297  tablenodes->set(
298  i, "id", format("%i", static_cast<int>(it->second->getID())));
299  tablenodes->set(i, "nodetype", it->second->m_nodeType);
300 
301  tablenodes->set(i, "annotation-list", ".");
303  it->second->m_annotations.begin();
304  ann != it->second->m_annotations.end(); ++ann)
305  {
306  size_t j = tableannots->appendRecord();
307  tableannots->set(
308  j, "id", format("%u", static_cast<unsigned int>(j)));
309  tableannots->set(j, "annotation-type", ann->name.c_str());
310  ASSERT_(ann->value);
311  string str;
312  if (IS_CLASS(ann->value, CPoint2D))
313  {
314  CPoint2D::Ptr o =
315  std::dynamic_pointer_cast<CPoint2D>(ann->value);
316  o->asString(str);
317  }
318  else
319  {
320  std::vector<uint8_t> v;
321  ObjectToOctetVector(ann->value.get(), v);
322  str.resize(v.size());
323  ::memcpy(&str[0], &v[0], v.size());
324  }
325  tableannots->set(j, "annotation-value", str);
326  if (tablenodes->get(j, "annotation-list") == ".")
327  tablenodes->set(
328  i, "annotation-list",
329  format("%u", static_cast<unsigned int>(j)));
330  else
331  tablenodes->set(
332  i, "annotation-list",
333  tablenodes->get(j, "annotation-list") +
334  format("%u", static_cast<unsigned int>(j)));
335  }
336  }
337 
338  // for arcs
339  printf("Generating arcs (%u)\n", static_cast<unsigned int>(m_arcs.size()));
340 
341  for (TArcList::const_iterator it = m_arcs.begin(); it != m_arcs.end(); ++it)
342  {
343  size_t fromid, toid;
344 
345  fromid = (int)(*it)->getNodeFrom();
346  toid = (int)(*it)->getNodeTo();
347 
348  i = tablearcs->appendRecord();
349  tablearcs->set(i, "id", format("%u", static_cast<unsigned int>(i)));
350  tablearcs->set(
351  i, "from", format("%u", static_cast<unsigned int>(fromid)));
352  tablearcs->set(i, "to", format("%u", static_cast<unsigned int>(toid)));
353  tablearcs->set(i, "arctype", (*it)->m_arcType);
354 
356  (*it)->m_annotations.begin();
357  ann != (*it)->m_annotations.end(); ++ann)
358  {
359  i = tableannots->appendRecord();
360  tableannots->set(
361  i, "id", format("%u", static_cast<unsigned int>(i)));
362  tableannots->set(i, "annotation-type", ann->name.c_str());
363 
364  // CSerializable *o=ann->value->clone(); // JL: duplicate???
365  // tableannots->set(i,"annotation-value",ObjectToString(o));
366  }
367  }
368  printf("Generating XML file\n");
369  db.saveAsXML(fileName.c_str());
370 
371  /*
372  std::string s;
373  s += format("NODE ID: %i\t LABEL:%s\tARCS: ",
374  (int)it->second->getID(), it->second->m_label.c_str() );
375  TArcList arcs;
376  it->second->getArcs(arcs);
377  for (TArcList::const_iterator a=arcs.begin();a!=arcs.end();++a)
378  s += format("%i-%i, ", (int)(*a)->getNodeFrom(),
379  (int)(*a)->getNodeTo() );
380 
381  st << s;
382 
383  for (CMHPropertiesValuesList::const_iterator ann =
384  it->second->m_annotations.begin(); ann !=
385  it->second->m_annotations.end(); ++ann)
386  {
387  s= format(" [HYPO ID #%02i] Annotation '%s' Class: ",
388  (int)ann->ID, ann->name.c_str() );
389  if ( ann->value )
390  s+= string(ann->value->GetRuntimeClass()->className);
391  else s+= "(nullptr)";
392 
393  st << s;
394 
395  if ( ann->name == NODE_ANNOTATION_REF_POSEID )
396  {
397  TPoseID refID;
398  it->second->m_annotations.getElemental(NODE_ANNOTATION_REF_POSEID,
399  refID, ann->ID );
400  st << format(" VALUE: %i",(int)refID);
401  }
402  else if ( ann->name == NODE_ANNOTATION_POSES_GRAPH )
403  {
404  CRobotPosesGraph::Ptr posesGraph =
405  it->second->m_annotations.getAs<CRobotPosesGraph>(NODE_ANNOTATION_POSES_GRAPH,ann->ID);
406  ASSERT_(posesGraph);
407 
408  st << format(" CRobotPosesGraph has %i
409  poses:",(int)posesGraph->size());
410  CPose3D pdfMean;
411  for (CRobotPosesGraph::const_iterator
412  p=posesGraph->begin();p!=posesGraph->end();++p)
413  {
414  const CPose3DPDFParticles &pdf = p->second.pdf;
415  pdf.getMean(pdfMean);
416  st << format(" Pose %i \t (%.03f,%.03f,%.03fdeg)",
417  (int)p->first,
418  pdfMean.x(),
419  pdfMean.y(),
420  RAD2DEG(pdfMean.yaw()));
421  }
422  }
423  }
424 
425  st << "";
426 
427  }
428 
429 
430  st << "";
431  st << "";
432  st << "LIST OF ARCS";
433  st << "================";
434 
435  for (TArcList::const_iterator it=m_arcs.begin();it!=m_arcs.end();++it)
436  {
437  std::string s;
438  s += format("ARC: %i -> %i\n", (int)(*it)->getNodeFrom(),
439  (int)(*it)->getNodeTo() );
440 
441  s+= string(" Arc type: ")+(*it)->m_arcType;
442 
443  st << s;
444 
445  for (CMHPropertiesValuesList::const_iterator ann =
446  (*it)->m_annotations.begin(); ann != (*it)->m_annotations.end(); ++ann)
447  {
448  s= format(" [HYPO ID #%02i] Annotation '%s' Class: ",
449  (int)ann->ID, ann->name.c_str() );
450  if ( ann->value )
451  s+= string(ann->value->GetRuntimeClass()->className);
452  else s+= "(nullptr)";
453 
454  st << s;
455 
456  if ( ann->name == ARC_ANNOTATION_DELTA_SRC_POSEID )
457  {
458  TPoseID refID;
459  (*it)->m_annotations.getElemental(ARC_ANNOTATION_DELTA_SRC_POSEID,
460  refID, ann->ID );
461  st << format(" VALUE: %i",(int)refID);
462  }
463  else if ( ann->name == ARC_ANNOTATION_DELTA_TRG_POSEID )
464  {
465  TPoseID refID;
466  (*it)->m_annotations.getElemental(ARC_ANNOTATION_DELTA_TRG_POSEID,
467  refID, ann->ID );
468  st << format(" VALUE: %i",(int)refID);
469  }
470  else if ( ann->name == ARC_ANNOTATION_DELTA )
471  {
472  CSerializable::Ptr o =
473  (*it)->m_annotations.get(ARC_ANNOTATION_DELTA, ann->ID );
474  ASSERT_(o);
475 
476  CPose3DPDFGaussian relativePoseAcordToArc;
477  relativePoseAcordToArc.copyFrom(*CPose3DPDF::Ptr(o));
478 
479  st << format(" VALUE: (%f,%f,%f , %fdeg,%fdeg,%fdeg)",
480  relativePoseAcordToArc.mean.x(),
481  relativePoseAcordToArc.mean.y(),
482  relativePoseAcordToArc.mean.z(),
483  RAD2DEG( relativePoseAcordToArc.mean.yaw() ),
484  RAD2DEG( relativePoseAcordToArc.mean.pitch() ),
485  RAD2DEG( relativePoseAcordToArc.mean.roll() ) );
486  }
487  }
488 
489  st << "";
490  }
491  */
492 }
Scalar * iterator
Definition: eigen_plugins.h:26
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)
This must be inserted in all CSerializable classes implementation files.
GLenum GLsizei n
Definition: glext.h:5074
GLenum GLsizei GLenum GLenum const GLvoid * table
Definition: glext.h:3531
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:156
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:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
A set of hypothesis IDs, used for arcs and nodes in multi-hypothesis hybrid maps. ...
std::vector< TPropertyValueIDTriplet >::const_iterator const_iterator
GLsizei const GLchar ** string
Definition: glext.h:4101
A class used to store a 2D point.
Definition: CPoint2D.h:33
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:3678
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:52
GLdouble GLdouble GLdouble r
Definition: glext.h:3705
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::rtti::CObject) is of the give...
Definition: CObject.h:102
GLuint in
Definition: glext.h:7274
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:3531
A class for representing an arc between two nodes in a hierarchical, multi-hypothesis map...
Definition: CHMHMapArc.h:29
GLsizei const GLfloat * value
Definition: glext.h:4117
#define NODE_ANNOTATION_PLACE_POSE
unsigned __int32 uint32_t
Definition: rptypes.h:47
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:186
const Scalar * const_iterator
Definition: eigen_plugins.h:27
GLuint GLuint GLsizei GLenum type
Definition: glext.h:3528
A class for representing a node in a hierarchical, multi-hypothesis map.
Definition: CHMHMapNode.h:34
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:356



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020