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



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