MRPT  1.9.9
CHierarchicalMapMHPartition.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/graphslam/levmarq.h>
16 #include <mrpt/opengl/CDisk.h>
20 #include <mrpt/opengl/CSphere.h>
21 #include <mrpt/opengl/CText.h>
23 #include <mrpt/random.h>
24 #include <mrpt/system/os.h>
25 
26 using namespace std;
27 using namespace mrpt;
28 using namespace mrpt::slam;
29 using namespace mrpt::random;
30 using namespace mrpt::hmtslam;
31 using namespace mrpt::system;
32 using namespace mrpt::serialization;
33 using namespace mrpt::poses;
34 using namespace mrpt::opengl;
35 using namespace mrpt::maps;
36 using namespace mrpt::math;
37 
38 /*---------------------------------------------------------------
39  nodeCount
40  ---------------------------------------------------------------*/
41 size_t CHierarchicalMapMHPartition::nodeCount() const { return m_nodes.size(); }
42 /*---------------------------------------------------------------
43  arcCount
44  ---------------------------------------------------------------*/
45 size_t CHierarchicalMapMHPartition::arcCount() const { return m_arcs.size(); }
46 /*---------------------------------------------------------------
47  getNodeByID
48  ---------------------------------------------------------------*/
49 CHMHMapNode::Ptr CHierarchicalMapMHPartition::getNodeByID(
51 {
53  if (id == AREAID_INVALID) return CHMHMapNode::Ptr();
54 
55  auto it = m_nodes.find(id);
56  return it == m_nodes.end() ? CHMHMapNode::Ptr() : it->second;
57 
58  MRPT_END
59 }
60 /*---------------------------------------------------------------
61  getNodeByID
62  ---------------------------------------------------------------*/
63 const CHMHMapNode::Ptr CHierarchicalMapMHPartition::getNodeByID(
64  CHMHMapNode::TNodeID id) const
65 {
67  if (id == AREAID_INVALID) return CHMHMapNode::Ptr();
68 
69  auto it = m_nodes.find(id);
70  return it == m_nodes.end() ? CHMHMapNode::Ptr() : it->second;
71 
72  MRPT_END
73 }
74 
75 /*---------------------------------------------------------------
76  getNodeByLabel
77  ---------------------------------------------------------------*/
78 CHMHMapNode::Ptr CHierarchicalMapMHPartition::getNodeByLabel(
79  const std::string& label, const THypothesisID& hypothesisID)
80 {
82 
83  // Look for the ID:
84  for (auto it = m_nodes.begin(); it != m_nodes.end(); ++it)
85  if (it->second->m_hypotheses.has(hypothesisID))
86  if (!os::_strcmpi(it->second->m_label.c_str(), label.c_str()))
87  return it->second;
88 
89  // Not found:
90  return CHMHMapNode::Ptr();
91 
92  MRPT_END
93 }
94 /*---------------------------------------------------------------
95  getNodeByLabel
96  ---------------------------------------------------------------*/
97 const CHMHMapNode::Ptr CHierarchicalMapMHPartition::getNodeByLabel(
98  const std::string& label, const THypothesisID& hypothesisID) const
99 {
100  MRPT_START
101 
102  // Look for the ID:
103  for (const auto& m_node : m_nodes)
104  if (m_node.second->m_hypotheses.has(hypothesisID))
105  if (!os::_strcmpi(m_node.second->m_label.c_str(), label.c_str()))
106  return m_node.second;
107 
108  // Not found:
109  return CHMHMapNode::Ptr();
110 
111  MRPT_END
112 }
113 
114 /*---------------------------------------------------------------
115  getFirstNode
116  ---------------------------------------------------------------*/
117 CHMHMapNode::Ptr CHierarchicalMapMHPartition::getFirstNode()
118 {
119  if (m_nodes.empty())
120  return CHMHMapNode::Ptr();
121  else
122  return (m_nodes.begin())->second;
123 }
124 
125 /*---------------------------------------------------------------
126  saveAreasDiagramForMATLAB
127  ---------------------------------------------------------------*/
128 void CHierarchicalMapMHPartition::saveAreasDiagramForMATLAB(
129  const std::string& filName, const CHMHMapNode::TNodeID& idReferenceNode,
130  const THypothesisID& hypothesisID) const
131 {
132  MRPT_UNUSED_PARAM(filName);
133  MRPT_UNUSED_PARAM(idReferenceNode);
134  MRPT_UNUSED_PARAM(hypothesisID);
135  /*
136  MRPT_START
137  unsigned int nAreaNodes=0;
138 
139  const CHMHMapNode *refNode = getNodeByID( idReferenceNode );
140  ASSERT_(refNode!=nullptr);
141  ASSERT_(refNode->nodeType.isType("Area"));
142 
143  FILE *f=os::fopen(filName.c_str(),"wt");
144  if (!f) THROW_EXCEPTION("Can not open output file!");
145 
146  // The list of all the m_nodes to be plotted:
147  // --------------------------------------------
148  std::map<CHMHMapNode::TNodeID,CPose2D> nodesPoses;
149  // The ref. pose of each area
150  std::map<CHMHMapNode::TNodeID,CPose2D> nodesMeanPoses;
151  // The mean pose of the observations in the area
152  std::map<CHMHMapNode::TNodeID,CPose2D>::iterator it;
153 
154  // First, insert the reference node:
155  nodesPoses[refNode->ID] = CPose2D(0,0,0);
156 
157  // Find the rest of "Area" m_nodes:
158  for (unsigned int i=0;i<nodeCount();i++)
159  {
160  const CHMHMapNode *nod = getNodeByIndex(i);
161 
162  // Is this a Area node?
163  if (nod->nodeType.isType("Area"))
164  {
165  nAreaNodes++; // Counter
166  if (nod!=refNode)
167  {
168  CPosePDFParticles posePDF;
169 
170  computeCoordinatesTransformationBetweenNodes(
171  refNode->ID,
172  nod->ID,
173  posePDF,
174  hypothesisID,
175  100);
176 
177  nodesPoses[nod->ID] = posePDF.getEstimatedPose();
178  }
179  }
180  } // end for each node "i"
181 
182  // Assure that all area m_nodes have been localized:
183  ASSERT_(nAreaNodes == nodesPoses.size() );
184 
185 
186  // Find the mean of the observations in each area:
187  for (it=nodesPoses.begin();it!=nodesPoses.end();it++)
188  {
189  CPose3D meanPose = it->second;
190  const CHMHMapNode *node = getNodeByID( it->first );
191 
192  CSimpleMap *localizedSFs = (CSimpleMap*)
193  node->annotations.get("localizedObservations");
194 
195  if (localizedSFs->size())
196  {
197  // Compute the mean pose:
198  CPose3D meanSFs(0,0,0);
199 
200  for (unsigned int k=0;k<localizedSFs->size();k++)
201  {
202  CPose3DPDF *pdf;
203  CSensoryFrame *dummy_sf;
204  CPose3D pdfMean;
205 
206  localizedSFs->get(k,pdf,dummy_sf);
207 
208  pdfMean = pdf->getEstimatedPose();
209  meanSFs.addComponents( pdfMean );
210  }
211  meanSFs *= 1.0f/(localizedSFs->size());
212  meanSFs.normalizeAngles();
213 
214  meanPose = meanPose + meanSFs;
215  }
216  else
217  {
218  // Let the ref. pose to represent the node
219  }
220 
221  nodesMeanPoses[it->first] = meanPose;
222  }
223 
224  // --------------------------------------------------------------
225  // Now we have the global poses of all the m_nodes: Draw them!
226  // -------------------------------------------------------------
227  // Header:
228  os::fprintf(f,"%%-------------------------------------------------------\n");
229  os::fprintf(f,"%% File automatically generated using the MRPT
230  method:\n");
231  os::fprintf(f,"%%'CHierarchicalMapMHPartition::saveAreasDiagramForMATLAB'\n");
232  os::fprintf(f,"%%\n");
233  os::fprintf(f,"%% ~ MRPT ~\n");
234  os::fprintf(f,"%% Jose Luis Blanco Claraco, University of Malaga @
235  2006\n");
236  os::fprintf(f,"%% http://www.isa.uma.es/ \n");
237  os::fprintf(f,"%%-------------------------------------------------------\n\n");
238 
239  os::fprintf(f,"hold on;\n");
240 
241  float nodesRadius = 1.5f;
242 
243  // Draw the m_nodes:
244  // ----------------------
245  for (it = nodesMeanPoses.begin();it!=nodesMeanPoses.end();it++)
246  {
247  const CHMHMapNode *node = getNodeByID( it->first );
248  CPose2D pose( it->second );
249 
250  os::fprintf(f,"\n%% Node: %s\n", node->label.c_str() );
251  os::fprintf(f,"rectangle('Curvature',[1 1],'Position',[%f %f %f
252  %f],'FaceColor',[1 1 1]);\n",
253  pose.x - nodesRadius,
254  pose.y - nodesRadius,
255  2*nodesRadius,
256  2*nodesRadius);
257 
258  // Draw the node's label:
259  os::fprintf(f,"text(%f,%f,'%s','FontSize',7,'HorizontalAlignment','center','Interpreter','none');\n",
260  pose.x,
261  pose.y,
262  node->label.c_str() );
263 
264  // And their m_arcs:
265  // ----------------------
266  for (unsigned int a=0;a<node->m_arcs.size();a++)
267  {
268  CHMHMapArc *arc = node->m_arcs[a];
269  if (arc->nodeFrom==node->ID)
270  {
271  CPose2D poseTo(
272  nodesMeanPoses.find(arc->nodeTo)->second );
273  float x1,x2,y1,y2, Ax,Ay;
274 
275  // Compute a unitary direction vector:
276  Ax = poseTo.x - pose.x;
277  Ay = poseTo.y - pose.y;
278 
279  float len = sqrt(square(Ax)+square(Ay));
280 
281  if (len>nodesRadius)
282  {
283  Ax /= len;
284  Ay /= len;
285 
286  x1 = pose.x + Ax * nodesRadius;
287  y1 = pose.y + Ay * nodesRadius;
288 
289  x2 = pose.x + Ax * (len-nodesRadius);
290  y2 = pose.y + Ay * (len-nodesRadius);
291 
292  os::fprintf(f,"line([%f %f],[%f,%f]);\n", x1,x2,y1,y2);
293  }
294  }
295  }
296  }
297 
298  os::fprintf(f,"axis equal; zoom on;");
299  os::fclose(f);
300  MRPT_END
301  */
302 }
303 
304 /*---------------------------------------------------------------
305  saveAreasDiagramWithEllipsedForMATLAB
306  ---------------------------------------------------------------*/
307 void CHierarchicalMapMHPartition::saveAreasDiagramWithEllipsedForMATLAB(
308  const std::string& filName, const CHMHMapNode::TNodeID& idReferenceNode,
309  const THypothesisID& hypothesisID, float uncertaintyExagerationFactor,
310  bool drawArcs, unsigned int numberOfIterationsForOptimalGlobalPoses) const
311 {
312  MRPT_UNUSED_PARAM(filName);
313  MRPT_UNUSED_PARAM(idReferenceNode);
314  MRPT_UNUSED_PARAM(hypothesisID);
315  MRPT_UNUSED_PARAM(uncertaintyExagerationFactor);
316  MRPT_UNUSED_PARAM(drawArcs);
317  MRPT_UNUSED_PARAM(numberOfIterationsForOptimalGlobalPoses);
318  /* MRPT_START
319 
320  const CHMHMapNode *refNode = getNodeByID( idReferenceNode );
321  ASSERT_(refNode!=nullptr);
322  ASSERT_(refNode->nodeType.isType("Area"));
323 
324  FILE *f=os::fopen(filName.c_str(),"wt");
325  if (!f) THROW_EXCEPTION("Can not open output file!");
326 
327  // The list of all the m_nodes to be plotted:
328  // --------------------------------------------
329  std::map<CHMHMapNode::TNodeID,CPosePDFGaussian> nodesPoses;
330  // The ref. pose of each area
331  std::map<CHMHMapNode::TNodeID,CPosePDFGaussian>::iterator it;
332  std::map<CHMHMapNode::TNodeID,CPosePDFGaussian> nodesMeanPoses;
333  // The mean pose of the observations in the area
334  std::map<CHMHMapNode::TNodeID,CPose2D>::iterator it2;
335 
336  computeGloballyConsistentNodeCoordinates( nodesPoses, idReferenceNode,
337  numberOfIterationsForOptimalGlobalPoses );
338 
339 
340  // Find the mean of the observations in each area:
341  for (it=nodesPoses.begin();it!=nodesPoses.end();it++)
342  {
343  //CPosePDFGaussian posePDF = it->second;
344  CPose2D meanPose = it->second.mean;
345 
346  const CHMHMapNode *node = getNodeByID( it->first );
347 
348  CSimpleMap *localizedSFs = (CSimpleMap*)
349  node->annotations.get("localizedObservations");
350 
351  if (localizedSFs->size())
352  {
353  // Compute the mean pose:
354  CPose3D meanSFs(0,0,0);
355 
356  for (unsigned int k=0;k<localizedSFs->size();k++)
357  {
358  CPose3DPDF *pdf;
359  CSensoryFrame *dummy_sf;
360  CPose3D pdfMean;
361 
362  localizedSFs->get(k,pdf,dummy_sf);
363 
364  pdfMean = pdf->getEstimatedPose();
365  meanSFs.addComponents( pdfMean );
366  }
367  meanSFs *= 1.0f/(localizedSFs->size());
368  meanSFs.normalizeAngles();
369 
370  meanPose = meanPose + meanSFs;
371  }
372  else
373  {
374  // Let the ref. pose to represent the node
375  }
376  nodesMeanPoses[it->first].mean = meanPose;
377  nodesMeanPoses[it->first].cov = it->second.cov;
378  }
379 
380  // --------------------------------------------------------------
381  // Now we have the global poses of all the m_nodes: Draw them!
382  // -------------------------------------------------------------
383  // Header:
384  os::fprintf(f,"%%-------------------------------------------------------\n");
385  os::fprintf(f,"%% File automatically generated using the MRPT
386  method:\n");
387  os::fprintf(f,"%%'CHierarchicalMapMHPartition::saveAreasDiagramWithEllipsedForMATLAB'\n");
388  os::fprintf(f,"%%\n");
389  os::fprintf(f,"%% ~ MRPT ~\n");
390  os::fprintf(f,"%% Jose Luis Blanco Claraco, University of Malaga @
391  2006\n");
392  os::fprintf(f,"%% http://www.isa.uma.es/ \n");
393  os::fprintf(f,"%%-------------------------------------------------------\n\n");
394 
395  os::fprintf(f,"hold on;\n");
396 
397  //float nodesRadius = 0; //2.0f;
398 
399  // Draw the m_nodes:
400  // ----------------------
401  for (it = nodesMeanPoses.begin();it!=nodesMeanPoses.end();it++)
402  {
403  const CHMHMapNode *node = getNodeByID( it->first );
404  CPosePDFGaussian posePDF = it->second;
405  CMatrixF C( posePDF.cov );
406  CPose2D pose( posePDF.mean );
407 
408  if (C.det()==0)
409  {
410  C.unit();
411  C(0,0)=1e-4f;
412  C(1,1)=1e-4f;
413  }
414 
415  os::fprintf(f,"\n%% Node: %s\n", node->label.c_str() );
416  os::fprintf(f,"c=[%e %e;%e %e];m=[%f
417  %f];error_ellipse(%f*c,m,'conf',0.95);\n",
418  C(0,0), C(0,1),
419  C(1,0), C(1,1),
420  pose.x,pose.y,
421  square(uncertaintyExagerationFactor) );
422 
423  // Draw the node's label:
424  os::fprintf(f,"text(%f,%f,'
425  %s','FontSize',8,'HorizontalAlignment','center','Interpreter','none');\n",
426  pose.x+1.5,
427  pose.y+1.5,
428  node->label.c_str() );
429  }
430 
431  // The m_arcs:
432  // ----------------------
433  if ( drawArcs )
434  for (it = nodesMeanPoses.begin();it!=nodesMeanPoses.end();it++)
435  {
436  const CHMHMapNode *node = getNodeByID( it->first );
437  CPose2D pose( it->second.mean );
438 
439  float nodesRadius = 0;
440  for (unsigned int a=0;a<node->m_arcs.size();a++)
441  {
442  CHMHMapArc *arc = node->m_arcs[a];
443  if (arc->nodeFrom==node->ID)
444  {
445  CPose2D poseTo(
446  nodesMeanPoses.find(arc->nodeTo)->second.mean );
447  float x1,x2,y1,y2, Ax,Ay;
448 
449  // Compute a unitary direction vector:
450  Ax = poseTo.x - pose.x;
451  Ay = poseTo.y - pose.y;
452 
453  float len = sqrt(square(Ax)+square(Ay));
454 
455  if (len>nodesRadius)
456  {
457  Ax /= len;
458  Ay /= len;
459 
460  x1 = pose.x + Ax * nodesRadius;
461  y1 = pose.y + Ay * nodesRadius;
462 
463  x2 = pose.x + Ax * (len-nodesRadius);
464  y2 = pose.y + Ay * (len-nodesRadius);
465 
466  os::fprintf(f,"line([%f %f],[%f,%f]);\n", x1,x2,y1,y2);
467  }
468  }
469  }
470  }
471 
472  os::fprintf(f,"axis equal; zoom on;");
473 
474  os::fclose(f);
475 
476  // Free memory:
477  nodesPoses.clear();
478 
479  MRPT_END
480  */
481 }
482 
483 /*---------------------------------------------------------------
484  saveGlobalMapForMATLAB
485  ---------------------------------------------------------------*/
486 void CHierarchicalMapMHPartition::saveGlobalMapForMATLAB(
487  const std::string& filName, const THypothesisID& hypothesisID,
488  const CHMHMapNode::TNodeID& idReferenceNode) const
489 {
490  MRPT_UNUSED_PARAM(filName);
491  MRPT_UNUSED_PARAM(hypothesisID);
492  MRPT_UNUSED_PARAM(idReferenceNode);
493  MRPT_START
494  /*
495  unsigned int nAreaNodes=0;
496 
497  const CHMHMapNode *refNode = getNodeByID( idReferenceNode );
498  ASSERT_(refNode!=nullptr);
499  ASSERT_(refNode->nodeType.isType("Area"));
500 
501  FILE *f=os::fopen(filName.c_str(),"wt");
502  if (!f) THROW_EXCEPTION("Can not open output file!");
503 
504  // The list of all the m_nodes to be plotted:
505  // --------------------------------------------
506  std::map<CHMHMapNode::TNodeID,CPose2D> nodesPoses;
507  // The ref. pose of each area
508  std::map<CHMHMapNode::TNodeID,CPose2D> nodesMeanPoses;
509  // The mean pose of the observations in the area
510  std::map<CHMHMapNode::TNodeID,CPose2D>::iterator it;
511 
512  // First, insert the reference node:
513  nodesPoses[refNode->ID] = CPose2D(0,0,0);
514 
515  // Find the rest of "Area" m_nodes:
516  for (unsigned int i=0;i<nodeCount();i++)
517  {
518  const CHMHMapNode *nod = getNodeByIndex(i);
519 
520  // Is this a Area node?
521  if (nod->nodeType.isType("Area"))
522  {
523  nAreaNodes++; // Counter
524  if (nod!=refNode)
525  {
526  CPosePDFParticles posePDF;
527 
528  computeCoordinatesTransformationBetweenNodes(
529  refNode->ID,
530  nod->ID,
531  posePDF,
532  hypothesisID,
533  100);
534 
535  nodesPoses[nod->ID] = posePDF.getEstimatedPose();
536  }
537  }
538  } // end for each node "i"
539 
540  // Assure that all area m_nodes have been localized:
541  ASSERT_(nAreaNodes == nodesPoses.size() );
542 
543  // Find the mean of the observations in each area:
544  for (it=nodesPoses.begin();it!=nodesPoses.end();it++)
545  {
546  CPose2D meanPose = it->second;
547  const CHMHMapNode *node = getNodeByID( it->first );
548 
549  CSimpleMap *localizedSFs = (CSimpleMap*)
550  node->annotations.get("localizedObservations");
551 
552  if (localizedSFs->size())
553  {
554  // Compute the mean pose:
555  CPose3D meanSFs(0,0,0);
556 
557  for (unsigned int k=0;k<localizedSFs->size();k++)
558  {
559  CPose3DPDF *pdf;
560  CSensoryFrame *dummy_sf;
561  CPose3D pdfMean;
562 
563  localizedSFs->get(k,pdf,dummy_sf);
564 
565  pdfMean = pdf->getEstimatedPose();
566  meanSFs.addComponents( pdfMean );
567  }
568  meanSFs *= 1.0f/(localizedSFs->size());
569  meanSFs.normalizeAngles();
570 
571  meanPose = meanPose + meanSFs;
572  }
573  else
574  {
575  // Let the ref. pose to represent the node
576  }
577 
578  nodesMeanPoses[it->first] = meanPose;
579  }
580 
581  // --------------------------------------------------------------
582  // Now we have the global poses of all the m_nodes: Draw them!
583  // -------------------------------------------------------------
584  // Header:
585  os::fprintf(f,"%%-------------------------------------------------------\n");
586  os::fprintf(f,"%% File automatically generated using the MRPT
587  method:\n");
588  os::fprintf(f,"%%'CHierarchicalMapMHPartition::saveAreasDiagramForMATLAB'\n");
589  os::fprintf(f,"%%\n");
590  os::fprintf(f,"%% ~ MRPT ~\n");
591  os::fprintf(f,"%% Jose Luis Blanco Claraco, University of Malaga @
592  2006\n");
593  os::fprintf(f,"%% http://www.isa.uma.es/ \n");
594  os::fprintf(f,"%%-------------------------------------------------------\n\n");
595 
596  os::fprintf(f,"hold on;\n");
597 
598  float nodesRadius = 1.5f;
599 
600  // Draw the metric maps of each area:
601  for (it = nodesPoses.begin();it!=nodesPoses.end();it++)
602  {
603  CPose2D meanPose = it->second;
604  const CHMHMapNode *node = getNodeByID( it->first );
605 
606  CMultiMetricMap *metricMap = (CMultiMetricMap*)
607  node->annotations.get("hybridMetricMap");
608  #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
609  ASSERT_(metricMap!=nullptr);
610  ASSERT_(metricMap->m_pointsMap);
611  #endif
612  int n;
613  float *xs,*ys,*zs;
614  CPoint2D pL,pG;
615 
616  metricMap->m_pointsMap->getPointsBuffer(n,xs,ys,zs);
617 
618  os::fprintf(f,"\n%% Metric map for node: %s\n", node->label.c_str()
619  );
620 
621  if (n)
622  {
623  os::fprintf(f,"map=[" );
624  for (int j=0;j<n;j+=10)
625  {
626  if (j!=0) os::fprintf(f,";" );
627  // Local coords:
628  pL.x = xs[j];
629  pL.y = ys[j];
630 
631  // Global coords:
632  pG = meanPose + pL;
633 
634  // write:
635  os::fprintf(f,"%.03f %.03f",pG.x,pG.y );
636  }
637  os::fprintf(f,"];\n" );
638  os::fprintf(f,"plot(map(:,1),map(:,2),'.k','MarkerSize',3);\n"
639  );
640  }
641 
642  }
643 
644 
645  // Draw the m_nodes:
646  // ----------------------
647  for (it = nodesMeanPoses.begin();it!=nodesMeanPoses.end();it++)
648  {
649  const CHMHMapNode *node = getNodeByID( it->first );
650  CPose2D pose( it->second );
651 
652  os::fprintf(f,"\n%% Node: %s\n", node->label.c_str() );
653  os::fprintf(f,"rectangle('Curvature',[1 1],'Position',[%f %f %f
654  %f],'FaceColor',[1 1 1]);\n",
655  pose.x - nodesRadius,
656  pose.y - nodesRadius,
657  2*nodesRadius,
658  2*nodesRadius);
659 
660  // Draw the node's label:
661  os::fprintf(f,"text(%f,%f,'%s','FontSize',7,'HorizontalAlignment','center','Interpreter','none');\n",
662  pose.x,
663  pose.y,
664  node->label.c_str() );
665 
666  // And their m_arcs:
667  // ----------------------
668  for (unsigned int a=0;a<node->m_arcs.size();a++)
669  {
670  CHMHMapArc *arc = node->m_arcs[a];
671  if (arc->nodeFrom==node->ID)
672  {
673  CPose2D poseTo(
674  nodesMeanPoses.find(arc->nodeTo)->second );
675  float x1,x2,y1,y2, Ax,Ay;
676 
677  // Compute a unitary direction vector:
678  Ax = poseTo.x - pose.x;
679  Ay = poseTo.y - pose.y;
680 
681  float len = sqrt(square(Ax)+square(Ay));
682 
683  if (len>nodesRadius)
684  {
685  Ax /= len;
686  Ay /= len;
687 
688  x1 = pose.x + Ax * nodesRadius;
689  y1 = pose.y + Ay * nodesRadius;
690 
691  x2 = pose.x + Ax * (len-nodesRadius);
692  y2 = pose.y + Ay * (len-nodesRadius);
693 
694  os::fprintf(f,"line([%f %f],[%f,%f]);\n", x1,x2,y1,y2);
695  }
696 
697  }
698  }
699 
700  }
701 
702 
703 
704  os::fprintf(f,"axis equal; zoom on;");
705 
706  os::fclose(f);
707  */
708  MRPT_END
709 }
710 
711 // Variables:
712 struct TDistance
713 {
714  TDistance() : dist(std::numeric_limits<unsigned>::max()) {}
715  TDistance(const unsigned& D) : dist(D) {}
716  const TDistance& operator=(const unsigned& D)
717  {
718  dist = D;
719  return *this;
720  }
721 
722  unsigned dist;
723 };
724 
725 struct TPrevious
726 {
729 };
730 
731 /*---------------------------------------------------------------
732  findPathBetweenNodes
733  ---------------------------------------------------------------*/
734 void CHierarchicalMapMHPartition::findPathBetweenNodes(
735  const CHMHMapNode::TNodeID& source, const CHMHMapNode::TNodeID& target,
736  const THypothesisID& hypothesisID, TArcList& ret, bool direction) const
737 {
738  MRPT_START
739 
740  /*
741  1 function Dijkstra(G, w, s)
742  2 for each vertex v in V[G] // Initializations
743  3 d[v] := infinity
744  4 previous[v] := undefined
745  5 d[s] := 0
746  6 S := empty set
747  7 Q := V[G]
748  8 while Q is not an empty set // The algorithm
749  itself
750  9 u := Extract_Min(Q)
751  ------> (u=t)? -> end!
752  10 S := S union {u}
753  11 for each edge (u,v) outgoing from u
754  12 if d[u] + w(u,v) < d[v] // Relax (u,v)
755  13 d[v] := d[u] + w(u,v)
756  14 previous[v] := u
757  */
758 
759  map<CHMHMapNode::TNodeID, TDistance> d; // distance
760  map<CHMHMapNode::TNodeID, TPrevious> previous;
761  map<CHMHMapNode::TNodeID, CHMHMapArc::Ptr> previous_arcs;
762  map<CHMHMapNode::TNodeID, bool> visited;
763 
764  unsigned visitedCount = 0;
765 
766  ASSERTMSG_(
767  m_nodes.find(source) != m_nodes.end(),
768  format("Source node %u not found in the H-Map", (unsigned int)source));
769  ASSERTMSG_(
770  m_nodes.find(target) != m_nodes.end(),
771  format("Target node %u not found in the H-Map", (unsigned int)target));
772 
773  ASSERT_(m_nodes.find(source)->second->m_hypotheses.has(hypothesisID));
774  ASSERT_(m_nodes.find(target)->second->m_hypotheses.has(hypothesisID));
775 
776  // Init:
777  // d: already initialized to infinity by default.
778  // previous: idem
779  // previous_arcs: idem
780  // visited: idem
781 
782  d[source] = 0;
783 
784  TNodeList::const_iterator u;
785 
786  // The algorithm:
787  do
788  {
789  // Finds the index of the minimum:
790  // unsigned int i;
791  unsigned min_d = std::numeric_limits<unsigned>::max();
792 
793  u = m_nodes.end();
794 
795  for (auto i = m_nodes.begin(); i != m_nodes.end(); ++i)
796  {
797  if (i->second->m_hypotheses.has(hypothesisID))
798  {
799  if (d[i->first].dist < min_d && !visited[i->first])
800  {
801  u = i;
802  min_d = d[u->first].dist;
803  }
804  }
805  }
806 
807  ASSERT_(u != m_nodes.end());
808 
809  visited[u->first] = true;
810  visitedCount++;
811 
812  // For each arc from "u":
813  const CHMHMapNode::Ptr nodeU = getNodeByID(u->first);
814  TArcList arcs;
815  nodeU->getArcs(arcs, hypothesisID);
816  for (auto i = arcs.begin(); i != arcs.end(); ++i)
817  {
819  if (!direction)
820  {
821  if ((*i)->getNodeFrom() != nodeU->getID())
822  vID = (*i)->getNodeFrom();
823  else
824  vID = (*i)->getNodeTo();
825  }
826  else
827  {
828  if ((*i)->getNodeFrom() != nodeU->getID())
829  continue;
830  else
831  vID = (*i)->getNodeTo();
832  }
833 
834  if ((min_d + 1) < d[vID].dist)
835  {
836  d[vID].dist = min_d + 1;
837  previous[vID].id = u->first;
838  previous_arcs[vID] = *i;
839  }
840  }
841  } while (u->first != target && visitedCount < m_nodes.size());
842 
843  // Arrived to target?
844  ret.clear();
845 
846  if (u->first == target)
847  {
848  // Path found:
849  CHMHMapNode::TNodeID nod = target;
850  do
851  {
852  ret.push_front(previous_arcs[nod]);
853  nod = previous[nod].id;
854  } while (nod != source);
855  }
856 
857  MRPT_END
858 }
859 
860 /*---------------------------------------------------------------
861  computeCoordinatesTransformationBetweenNodes
862  ---------------------------------------------------------------*/
863 void CHierarchicalMapMHPartition::computeCoordinatesTransformationBetweenNodes(
864  const CHMHMapNode::TNodeID& nodeFrom, const CHMHMapNode::TNodeID& nodeTo,
865  CPose3DPDFParticles& posePDF, const THypothesisID& hypothesisID,
866  unsigned int particlesCount, float additionalNoiseXYratio,
867  float additionalNoisePhiRad) const
868 {
869  MRPT_START
870 
871  TArcList arcsPath;
872  TArcList::const_iterator arcsIt;
873  const TPose3D nullPose(0, 0, 0, 0, 0, 0);
874  CHMHMapNode::TNodeID lastNode, nextNode;
875  size_t pathLength;
876 
877  using TPose3DList = std::vector<CPose3D>;
878  std::vector<TPose3DList> listSamples;
879  std::vector<TPose3DList>::iterator lstIt;
880  TPose3DList dummyList;
881  TPose3DList::iterator poseIt;
882 
883  // Find the shortest sequence of arcs connecting the nodes:
884  findPathBetweenNodes(nodeFrom, nodeTo, hypothesisID, arcsPath);
885  ASSERT_(arcsPath.size());
886  pathLength = arcsPath.size();
887 
888  // Prepare the PDF:
889  posePDF.resetDeterministic(nullPose, particlesCount);
890 
891  // Draw the pose sample:
892  lastNode = nodeFrom;
893  dummyList.resize(particlesCount);
894  listSamples.resize(pathLength, dummyList);
895  for (arcsIt = arcsPath.begin(), lstIt = listSamples.begin();
896  arcsIt != arcsPath.end(); lstIt++, arcsIt++)
897  {
898  // Flag for the pose PDF needing to be reversed:
899  bool reversedArc = (*arcsIt)->getNodeTo() == lastNode;
900  nextNode =
901  reversedArc ? (*arcsIt)->getNodeFrom() : (*arcsIt)->getNodeTo();
902 
903  // Check reference pose IDs between arcs:
904  // ------------------------------------------------
905  TPoseID curNodeRefPoseID, nextNodeRefPoseID;
906  getNodeByID((*arcsIt)->getNodeFrom())
907  ->m_annotations.getElemental(
908  NODE_ANNOTATION_REF_POSEID, curNodeRefPoseID, hypothesisID,
909  true);
910  getNodeByID((*arcsIt)->getNodeTo())
911  ->m_annotations.getElemental(
912  NODE_ANNOTATION_REF_POSEID, nextNodeRefPoseID, hypothesisID,
913  true);
914 
915  TPoseID srcRefPoseID, trgRefPoseID;
916  (*arcsIt)->m_annotations.getElemental(
917  ARC_ANNOTATION_DELTA_SRC_POSEID, srcRefPoseID, hypothesisID, true);
918  (*arcsIt)->m_annotations.getElemental(
919  ARC_ANNOTATION_DELTA_TRG_POSEID, trgRefPoseID, hypothesisID, true);
920 
921  ASSERT_(curNodeRefPoseID == srcRefPoseID);
922  ASSERT_(nextNodeRefPoseID == trgRefPoseID);
923 
924  // Get the pose PDF:
925  CSerializable::Ptr anotation =
926  (*arcsIt)->m_annotations.get(ARC_ANNOTATION_DELTA, hypothesisID);
927  ASSERT_(anotation);
928 
929  CPose3DPDFGaussian pdf; // Convert to gaussian
930  pdf.copyFrom(*std::dynamic_pointer_cast<CPose3DPDF>(anotation));
931 
932  vector<CVectorDouble> samples;
933 
934  pdf.drawManySamples(lstIt->size(), samples);
935  ASSERT_(samples.size() == lstIt->size());
936  ASSERT_(samples[0].size() == 6);
937 
938  vector<CVectorDouble>::const_iterator samplIt;
939  for (poseIt = lstIt->begin(), samplIt = samples.begin();
940  poseIt != lstIt->end(); poseIt++, samplIt++)
941  {
942  // Minimum added noise:
943  poseIt->setFromValues(
944  (*samplIt)[0] +
945  additionalNoiseXYratio *
946  getRandomGenerator().drawGaussian1D_normalized(),
947  (*samplIt)[1] +
948  additionalNoiseXYratio *
949  getRandomGenerator().drawGaussian1D_normalized(),
950  (*samplIt)[2],
951  (*samplIt)[3] +
952  additionalNoisePhiRad *
953  getRandomGenerator().drawGaussian1D_normalized(),
954  (*samplIt)[4], (*samplIt)[5]);
955 
956  // Pose composition:
957  if (reversedArc) *poseIt = (CPose3D() - CPose3D(*poseIt));
958  }
959 
960  // for the next iter:
961  lastNode = nextNode;
962  }
963 
964  // Compute the pose composition:
965  for (unsigned int i = 0; i < particlesCount; i++)
966  {
967  CPose3D sample = CPose3D(posePDF.m_particles[i].d);
968 
969  for (unsigned int j = 0; j < pathLength; j++)
970  {
971  if (j)
972  sample = sample + listSamples[j][i];
973  else
974  sample = listSamples[j][i];
975  }
976  }
977 
978  posePDF.normalizeWeights();
979 
980 #if 0
981  CPose3DPDFGaussian auxPDF;
982  auxPDF.copyFrom( posePDF );
983  cout << "[computeCoordinatesTransformationBetweenNodes] Nodes: " << nodeFrom << " - " << nodeTo << ": " << auxPDF;
984 #endif
985 
986  MRPT_END
987 }
988 
989 /*---------------------------------------------------------------
990  computeMatchProbabilityBetweenNodes
991  ---------------------------------------------------------------*/
992 float CHierarchicalMapMHPartition::computeMatchProbabilityBetweenNodes(
993  const CHMHMapNode::TNodeID& nodeFrom, const CHMHMapNode::TNodeID& nodeTo,
994  float& maxMatchProb, CPose3DPDFSOG& estimatedRelativePose,
995  const THypothesisID& hypothesisID, unsigned int monteCarloSamplesPose)
996 {
997  MRPT_UNUSED_PARAM(nodeFrom);
998  MRPT_UNUSED_PARAM(nodeTo);
999  MRPT_UNUSED_PARAM(maxMatchProb);
1000  MRPT_UNUSED_PARAM(estimatedRelativePose);
1001  MRPT_UNUSED_PARAM(hypothesisID);
1002  MRPT_UNUSED_PARAM(monteCarloSamplesPose);
1003  MRPT_START
1004  THROW_EXCEPTION("TO DO");
1005  MRPT_END
1006 }
1007 
1008 /*---------------------------------------------------------------
1009  findArcsBetweenNodes
1010  ---------------------------------------------------------------*/
1011 void CHierarchicalMapMHPartition::findArcsBetweenNodes(
1012  const CHMHMapNode::TNodeID& node1id, const CHMHMapNode::TNodeID& node2id,
1013  const THypothesisID& hypothesisID, TArcList& ret) const
1014 {
1015  MRPT_START
1016 
1017  ret.clear();
1018  const CHMHMapNode::Ptr node1 = getNodeByID(node1id);
1019 
1020  TArcList lstArcs;
1021  TArcList::const_iterator itArc;
1022 
1023  node1->getArcs(lstArcs);
1024  for (itArc = lstArcs.begin(); itArc != lstArcs.end(); itArc++)
1025  {
1026  if ((*itArc)->m_hypotheses.has(hypothesisID))
1027  if ((*itArc)->m_nodeFrom == node2id ||
1028  (*itArc)->m_nodeTo == node2id)
1029  {
1030  ret.push_back(*itArc);
1031  }
1032  }
1033 
1034  MRPT_END
1035 }
1036 
1037 /*---------------------------------------------------------------
1038  findArcsOfTypeBetweenNodes
1039  ---------------------------------------------------------------*/
1040 void CHierarchicalMapMHPartition::findArcsOfTypeBetweenNodes(
1041  const CHMHMapNode::TNodeID& node1id, const CHMHMapNode::TNodeID& node2id,
1042  const THypothesisID& hypothesisID, const std::string& arcType,
1043  TArcList& ret) const
1044 {
1045  MRPT_START
1046 
1047  ret.clear();
1048  const CHMHMapNode::Ptr node1 = getNodeByID(node1id);
1049 
1050  TArcList lstArcs;
1051  TArcList::const_iterator itArc;
1052 
1053  node1->getArcs(lstArcs);
1054  for (itArc = lstArcs.begin(); itArc != lstArcs.end(); itArc++)
1055  {
1056  if ((*itArc)->m_hypotheses.has(hypothesisID))
1057  if ((*itArc)->m_nodeFrom == node2id ||
1058  (*itArc)->m_nodeTo == node2id)
1059  {
1060  if ((*itArc)->m_arcType == arcType) ret.push_back(*itArc);
1061  }
1062  }
1063 
1064  MRPT_END
1065 }
1066 
1067 /*---------------------------------------------------------------
1068  areNodesNeightbour
1069  ---------------------------------------------------------------*/
1070 bool CHierarchicalMapMHPartition::areNodesNeightbour(
1071  const CHMHMapNode::TNodeID& node1id, const CHMHMapNode::TNodeID& node2id,
1072  const THypothesisID& hypothesisID, const char* requiredAnnotation) const
1073 {
1074  MRPT_START
1075 
1076  const CHMHMapNode::Ptr node1 = getNodeByID(node1id);
1077 
1078  TArcList lstArcs;
1079  TArcList::const_iterator itArc;
1080 
1081  node1->getArcs(lstArcs);
1082  for (itArc = lstArcs.begin(); itArc != lstArcs.end(); itArc++)
1083  {
1084  if ((*itArc)->m_hypotheses.has(hypothesisID))
1085  if ((*itArc)->m_nodeFrom == node2id ||
1086  (*itArc)->m_nodeTo == node2id)
1087  {
1088  if (!requiredAnnotation)
1089  return true;
1090  else if ((*itArc)->m_annotations.get(
1091  requiredAnnotation, hypothesisID))
1092  return true;
1093  }
1094  }
1095 
1096  return false;
1097 
1098  MRPT_END
1099 }
1100 
1101 /*---------------------------------------------------------------
1102  getAs3DScene
1103  ---------------------------------------------------------------*/
1104 
1105 void CHierarchicalMapMHPartition::getAs3DScene(
1106  COpenGLScene& outScene, const CHMHMapNode::TNodeID& idReferenceNode,
1107  const THypothesisID& hypothesisID,
1108  const unsigned int& numberOfIterationsForOptimalGlobalPoses,
1109  const bool& showRobotPoseIDs) const
1110 {
1111  MRPT_START
1112 
1113  // First: Clear and add the cool "ground grid" :-P
1114  outScene.clear();
1115  {
1117  mrpt::opengl::CGridPlaneXY::Create(-500, 500, -500, 500, 0, 5);
1118  obj->setColor(0.3, 0.3, 0.3);
1119  outScene.insert(obj);
1120  }
1121 
1122  using TMapID2PosePDF = std::map<CHMHMapNode::TNodeID, CPose3DPDFGaussian>;
1123  // The ref. pose of each area
1124  TMapID2PosePDF nodesPoses;
1125  TMapID2PosePDF::iterator it;
1126 
1127  using TMapID2Pose2D = std::map<CHMHMapNode::TNodeID, CPose2D>;
1128  // The mean pose of the observations in the area
1129  TMapID2Pose2D nodesMeanPoses;
1130  TMapID2Pose2D::iterator it2;
1131 
1132  // Only those nodes in the "hypothesisID" are computed.
1133  computeGloballyConsistentNodeCoordinates(
1134  nodesPoses, idReferenceNode, hypothesisID,
1135  numberOfIterationsForOptimalGlobalPoses);
1136 
1137  // Find the mean of the observations in each area:
1138  for (it = nodesPoses.begin(); it != nodesPoses.end(); it++)
1139  {
1140  CPose2D meanPose = CPose2D(it->second.mean);
1141  const CHMHMapNode::Ptr node = getNodeByID(it->first);
1142 
1143  CRobotPosesGraph::Ptr posesGraph =
1144  node->m_annotations.getAs<CRobotPosesGraph>(
1145  NODE_ANNOTATION_POSES_GRAPH, hypothesisID);
1146 
1147  if (posesGraph && posesGraph->size())
1148  {
1149  // Compute the mean pose:
1150  CPose3D meanSFs(0, 0, 0);
1151 
1152  for (auto it = posesGraph->begin(); it != posesGraph->end(); ++it)
1153  meanSFs.addComponents(it->second.pdf.getMeanVal());
1154 
1155  meanSFs *= 1.0f / (posesGraph->size());
1156  meanSFs.normalizeAngles();
1157 
1158  meanPose = meanPose + CPose2D(meanSFs);
1159  }
1160  else
1161  {
1162  // Let the ref. pose to represent the node
1163  }
1164 
1165  nodesMeanPoses[it->first] = meanPose;
1166  }
1167 
1168  // ----------------------------------------------------------------
1169  // Now we have the global poses of all the m_nodes: Draw'em all
1170  // ---------------------------------------------------------------
1171  // Draw the (grid maps) metric maps of each area:
1172  for (it = nodesPoses.begin(); it != nodesPoses.end(); it++)
1173  {
1174  const CPose3D& pose = it->second.mean;
1175  const CHMHMapNode::Ptr node = getNodeByID(it->first);
1176 
1177  CMultiMetricMap::Ptr metricMap =
1178  node->m_annotations.getAs<CMultiMetricMap>(
1179  NODE_ANNOTATION_METRIC_MAPS, hypothesisID);
1180  if (metricMap) // ASSERT_(metricMap);
1181  {
1184  metricMap->getAs3DObject(objTex);
1185  objTex->setPose(pose);
1186  outScene.insert(objTex);
1187  }
1188  }
1189 
1190  float nodes_height = 10.0f;
1191  float nodes_radius = 1.0f;
1192 
1193  // Draw the m_nodes:
1194  // ----------------------
1195  for (it = nodesPoses.begin(), it2 = nodesMeanPoses.begin();
1196  it != nodesPoses.end(); it++, it2++)
1197  {
1198  const CHMHMapNode::Ptr node = getNodeByID(it->first);
1199  const CPose3D& pose = it->second.mean;
1200  const CPose3D meanPose = CPose3D(it2->second);
1201 
1202  // The sphere of the node:
1204 
1205  objSphere->setName(node->m_label);
1206  objSphere->setColor(0, 0, 1);
1207  objSphere->setLocation(
1208  meanPose.x(), meanPose.y(), meanPose.z() + nodes_height);
1209  objSphere->setRadius(nodes_radius);
1210  objSphere->setNumberDivsLongitude(16);
1211  objSphere->setNumberDivsLatitude(16);
1212 
1213  objSphere->enableRadiusIndependentOfEyeDistance();
1214 
1215  outScene.insert(objSphere);
1216 
1217  // The label with the name of the node:
1219  // objText->m_str = node->m_label;
1220  objText->setString(format("%li", (long int)node->getID()));
1221  // objText->m_fontHeight = 20;
1222  objText->setColor(1, 0, 0);
1223  objText->setLocation(
1224  meanPose.x(), meanPose.y(), meanPose.z() + nodes_height);
1225 
1226  outScene.insert(objText);
1227 
1228  // And the observations "on the ground" as disks
1229  // ---------------------------------------------------
1230  CRobotPosesGraph::Ptr posesGraph =
1231  node->m_annotations.getAs<CRobotPosesGraph>(
1232  NODE_ANNOTATION_POSES_GRAPH, hypothesisID);
1233  // ASSERT_(posesGraph);
1234 
1235  if (posesGraph)
1236  {
1237  for (auto it = posesGraph->begin(); it != posesGraph->end(); ++it)
1238  {
1239  CPose3D SF_pose;
1240  it->second.pdf.getMean(SF_pose);
1241 
1242  CPose3D auxPose(pose + SF_pose);
1243 
1245 
1246  glObj->setColor(1, 0, 0);
1247 
1248  glObj->setPose(auxPose);
1249  // glObj->m_z = 0;
1250 
1251  glObj->setDiskRadius(0.05f);
1252  glObj->setSlicesCount(20);
1253  glObj->setLoopsCount(10);
1254 
1255  if (showRobotPoseIDs)
1256  {
1257  glObj->setName(format("%i", (int)it->first));
1258  glObj->enableShowName();
1259  }
1260 
1261  outScene.insert(glObj);
1262 
1263  // And a line up-to the node:
1266 
1267  objLine->setColor(1, 0, 0, 0.2);
1268  objLine->setLineWidth(1.5);
1269 
1270  objLine->setLineCoords(
1271  meanPose.x(), meanPose.y(), nodes_height, auxPose.x(),
1272  auxPose.y(), 0);
1273 
1274  outScene.insert(objLine);
1275  } // end for observation "disks"
1276  }
1277 
1278  // And their m_arcs:
1279  // ----------------------
1280  TArcList arcs;
1281  node->getArcs(arcs, hypothesisID);
1282  for (auto a = arcs.begin(); a != arcs.end(); ++a)
1283  {
1284  CHMHMapArc::Ptr arc = *a;
1285 
1286  if (arc->getNodeFrom() == node->getID())
1287  {
1288  CPose3D poseTo(nodesMeanPoses.find(arc->getNodeTo())->second);
1289 
1290  // Draw the line:
1293 
1294  objLine->setColor(0, 1, 0, 0.5);
1295  objLine->setLineWidth(5);
1296 
1297  objLine->setLineCoords(
1298  meanPose.x(), meanPose.y(), meanPose.z() + nodes_height,
1299  poseTo.x(), poseTo.y(), poseTo.z() + nodes_height);
1300 
1301  outScene.insert(objLine);
1302  }
1303  }
1304  }
1305 
1306  MRPT_END
1307 }
1308 
1309 void CHierarchicalMapMHPartition::computeGloballyConsistentNodeCoordinates(
1310  std::map<CHMHMapNode::TNodeID, mrpt::poses::CPose3DPDFGaussian>& nodePoses,
1311  const CHMHMapNode::TNodeID& idReferenceNode,
1312  const THypothesisID& hypothesisID,
1313  const unsigned int& numberOfIterations) const
1314 {
1315  MRPT_START
1316 
1317  nodePoses.clear();
1318 
1319  if (m_arcs.empty()) return; // Nothing we can do!
1320 
1321  // 1) Convert hmt-slam graph into graphslam graph... (this should be avoided
1322  // in future version of HTML-SLAM!!)
1323  graphs::CNetworkOfPoses3DInf pose_graph;
1324 
1325  for (const auto& m_arc : m_arcs)
1326  {
1327  if (!m_arc->m_hypotheses.has(hypothesisID)) continue;
1328 
1329  const CHMHMapNode::TNodeID id_from = m_arc->getNodeFrom();
1330  const CHMHMapNode::TNodeID id_to = m_arc->getNodeTo();
1331 
1332  CSerializable::Ptr anotation =
1333  m_arc->m_annotations.get(ARC_ANNOTATION_DELTA, hypothesisID);
1334  if (!anotation) continue;
1335 
1336  CPose3DPDFGaussianInf edge_rel_pose_pdf; // Convert to gaussian
1337  edge_rel_pose_pdf.copyFrom(
1338  *std::dynamic_pointer_cast<CPose3DPDF>(anotation));
1339 
1340  pose_graph.insertEdgeAtEnd(id_from, id_to, edge_rel_pose_pdf);
1341  }
1342 
1343  // 2) Initialize global poses of nodes with Dijkstra:
1344  pose_graph.root = idReferenceNode;
1345  pose_graph.dijkstra_nodes_estimate();
1346 
1347  // 3) Optimize with graph-slam:
1349  TParametersDouble graphslam_params;
1350  graphslam_params["max_iterations"] = numberOfIterations;
1351 
1353  pose_graph, out_info,
1354  nullptr, // Optimize all nodes
1355  graphslam_params);
1356 
1357  // 4) Copy back optimized results into the HMT-SLAM graph:
1358  for (auto it_node = pose_graph.nodes.begin();
1359  it_node != pose_graph.nodes.end(); ++it_node)
1360  {
1361  const CHMHMapNode::TNodeID node_id = it_node->first;
1362 
1363  // To the output map:
1364  CPose3DPDFGaussian& new_pose = nodePoses[node_id];
1365  new_pose.mean = it_node->second;
1366  new_pose.cov.setIdentity(); // *** At present, graphslam does not
1367  // output the uncertainty of poses... ***
1368  }
1369 
1370 #if __computeGloballyConsistentNodeCoordinates__VERBOSE
1371  for (map<CHMHMapNode::TNodeID, CPose3DPDFGaussian>::const_iterator it =
1372  nodePoses.begin();
1373  it != nodePoses.end(); ++it)
1374  cout << it->first << ": " << it->second.mean << endl;
1375  cout << endl;
1376 #endif
1377 
1378  MRPT_END
1379 }
1380 
1381 /*---------------------------------------------------------------
1382  dumpAsText
1383  ---------------------------------------------------------------*/
1384 void CHierarchicalMapMHPartition::dumpAsText(std::vector<std::string>& st) const
1385 {
1386  st.clear();
1387  st.emplace_back("LIST OF NODES");
1388  st.emplace_back("================");
1389 
1390  for (const auto& m_node : m_nodes)
1391  {
1392  std::string s;
1393  s += format(
1394  "NODE ID: %i\t LABEL:%s\tARCS: ", (int)m_node.second->getID(),
1395  m_node.second->m_label.c_str());
1396  TArcList arcs;
1397  m_node.second->getArcs(arcs);
1398  for (auto a = arcs.begin(); a != arcs.end(); ++a)
1399  s += format(
1400  "%i-%i, ", (int)(*a)->getNodeFrom(), (int)(*a)->getNodeTo());
1401 
1402  st.push_back(s);
1403 
1404  for (auto ann = m_node.second->m_annotations.begin();
1405  ann != m_node.second->m_annotations.end(); ++ann)
1406  {
1407  s = format(
1408  " [HYPO ID #%02i] Annotation '%s' Class: ", (int)ann->ID,
1409  ann->name.c_str());
1410  if (ann->value)
1411  s += string(ann->value->GetRuntimeClass()->className);
1412  else
1413  s += "(nullptr)";
1414 
1415  st.push_back(s);
1416 
1417  if (ann->name == NODE_ANNOTATION_REF_POSEID)
1418  {
1419  TPoseID refID;
1420  m_node.second->m_annotations.getElemental(
1421  NODE_ANNOTATION_REF_POSEID, refID, ann->ID);
1422  st.push_back(format(" VALUE: %i", (int)refID));
1423  }
1424  else if (ann->name == NODE_ANNOTATION_POSES_GRAPH)
1425  {
1426  CRobotPosesGraph::Ptr posesGraph =
1427  m_node.second->m_annotations.getAs<CRobotPosesGraph>(
1428  NODE_ANNOTATION_POSES_GRAPH, ann->ID);
1429  ASSERT_(posesGraph);
1430 
1431  st.push_back(format(
1432  " CRobotPosesGraph has %i poses:",
1433  (int)posesGraph->size()));
1434  CPose3D pdfMean;
1435  for (auto p = posesGraph->begin(); p != posesGraph->end(); ++p)
1436  {
1437  const CPose3DPDFParticles& pdf = p->second.pdf;
1438  pdf.getMean(pdfMean);
1439  st.push_back(format(
1440  " Pose %i \t (%.03f,%.03f,%.03fdeg)",
1441  (int)p->first, pdfMean.x(), pdfMean.y(),
1442  RAD2DEG(pdfMean.yaw())));
1443  }
1444  }
1445  }
1446 
1447  st.emplace_back("");
1448  }
1449 
1450  st.emplace_back("");
1451  st.emplace_back("");
1452  st.emplace_back("LIST OF ARCS");
1453  st.emplace_back("================");
1454 
1455  for (const auto& m_arc : m_arcs)
1456  {
1457  std::string s;
1458  s += format(
1459  "ARC: %i -> %i\n", (int)m_arc->getNodeFrom(),
1460  (int)m_arc->getNodeTo());
1461 
1462  s += string(" Arc type: ") + m_arc->m_arcType;
1463 
1464  st.push_back(s);
1465 
1466  for (auto ann = m_arc->m_annotations.begin();
1467  ann != m_arc->m_annotations.end(); ++ann)
1468  {
1469  s = format(
1470  " [HYPO ID #%02i] Annotation '%s' Class: ", (int)ann->ID,
1471  ann->name.c_str());
1472  if (ann->value)
1473  s += string(ann->value->GetRuntimeClass()->className);
1474  else
1475  s += "(nullptr)";
1476 
1477  st.push_back(s);
1478 
1479  if (ann->name == ARC_ANNOTATION_DELTA_SRC_POSEID)
1480  {
1481  TPoseID refID;
1482  m_arc->m_annotations.getElemental(
1483  ARC_ANNOTATION_DELTA_SRC_POSEID, refID, ann->ID);
1484  st.push_back(format(" VALUE: %i", (int)refID));
1485  }
1486  else if (ann->name == ARC_ANNOTATION_DELTA_TRG_POSEID)
1487  {
1488  TPoseID refID;
1489  m_arc->m_annotations.getElemental(
1490  ARC_ANNOTATION_DELTA_TRG_POSEID, refID, ann->ID);
1491  st.push_back(format(" VALUE: %i", (int)refID));
1492  }
1493  else if (ann->name == ARC_ANNOTATION_DELTA)
1494  {
1495  CSerializable::Ptr o =
1496  m_arc->m_annotations.get(ARC_ANNOTATION_DELTA, ann->ID);
1497  ASSERT_(o);
1498 
1499  CPose3DPDFGaussian relativePoseAcordToArc;
1500  relativePoseAcordToArc.copyFrom(
1501  *std::dynamic_pointer_cast<CPose3DPDF>(o));
1502 
1503  st.push_back(format(
1504  " VALUE: (%f,%f,%f , %fdeg,%fdeg,%fdeg)",
1505  relativePoseAcordToArc.mean.x(),
1506  relativePoseAcordToArc.mean.y(),
1507  relativePoseAcordToArc.mean.z(),
1508  RAD2DEG(relativePoseAcordToArc.mean.yaw()),
1509  RAD2DEG(relativePoseAcordToArc.mean.pitch()),
1510  RAD2DEG(relativePoseAcordToArc.mean.roll())));
1511  }
1512  }
1513 
1514  st.emplace_back("");
1515  }
1516 }
1517 
1518 /*---------------------------------------------------------------
1519  findArcOfTypeBetweenNodes
1520  ---------------------------------------------------------------*/
1521 CHMHMapArc::Ptr CHierarchicalMapMHPartition::findArcOfTypeBetweenNodes(
1522  const CHMHMapNode::TNodeID& node1id, const CHMHMapNode::TNodeID& node2id,
1523  const THypothesisID& hypothesisID, const std::string& arcType,
1524  bool& isInverted) const
1525 {
1526  MRPT_START
1527 
1528  TArcList lstArcs;
1529  findArcsOfTypeBetweenNodes(
1530  node1id, node2id, hypothesisID, arcType, lstArcs);
1531 
1532  for (auto a = lstArcs.begin(); a != lstArcs.end(); ++a)
1533  {
1534  if ((*a)->getNodeFrom() == node1id)
1535  {
1536  // Found, and in the correct direction:
1537  isInverted = false;
1538  return *a;
1539  }
1540  else
1541  {
1542  // Found, in the opossite direction:
1543  isInverted = true;
1544  return *a;
1545  }
1546  }
1547 
1548  return CHMHMapArc::Ptr();
1549  MRPT_END
1550 }
1551 
1552 /*---------------------------------------------------------------
1553  computeOverlapProbabilityBetweenNodes
1554  ---------------------------------------------------------------*/
1555 double CHierarchicalMapMHPartition::computeOverlapProbabilityBetweenNodes(
1556  const CHMHMapNode::TNodeID& nodeFrom, const CHMHMapNode::TNodeID& nodeTo,
1557  const THypothesisID& hypothesisID, const size_t& monteCarloSamples,
1558  const float margin_to_substract) const
1559 {
1560  MRPT_START
1561 
1562  size_t i, hits = 0;
1563  CPose3DPDFParticles posePDF;
1564  const CHMHMapNode::Ptr from = getNodeByID(nodeFrom);
1565  const CHMHMapNode::Ptr to = getNodeByID(nodeTo);
1566 
1567  // Draw pose samples:
1568  computeCoordinatesTransformationBetweenNodes(
1569  nodeFrom, nodeTo, posePDF, hypothesisID, monteCarloSamples);
1570  /*0.15f,
1571  DEG2RAD(3.0f) );*/
1572 
1573  // Get the extends of grid maps:
1574  float r1_x_min, r1_x_max, r1_y_min, r1_y_max, r2_x_min, r2_x_max, r2_y_min,
1575  r2_y_max;
1576 
1577  // MAP1:
1578  CMultiMetricMap::Ptr hMap1 = from->m_annotations.getAs<CMultiMetricMap>(
1579  NODE_ANNOTATION_METRIC_MAPS, hypothesisID, false);
1580 
1581  auto grid = hMap1->mapByClass<COccupancyGridMap2D>();
1582  ASSERT_(grid);
1583 
1584  r1_x_min = grid->getXMin();
1585  r1_x_max = grid->getXMax();
1586  r1_y_min = grid->getYMin();
1587  r1_y_max = grid->getYMax();
1588 
1589  if (r1_x_max - r1_x_min > 2 * margin_to_substract)
1590  {
1591  r1_x_max -= margin_to_substract;
1592  r1_x_min += margin_to_substract;
1593  }
1594  if (r1_y_max - r1_y_min > 2 * margin_to_substract)
1595  {
1596  r1_y_max -= margin_to_substract;
1597  r1_y_min += margin_to_substract;
1598  }
1599 
1600  // MAP2:
1601  CMultiMetricMap::Ptr hMap2 = to->m_annotations.getAs<CMultiMetricMap>(
1602  NODE_ANNOTATION_METRIC_MAPS, hypothesisID, false);
1603 
1604  auto grid2 = hMap2->mapByClass<COccupancyGridMap2D>();
1605  ASSERT_(grid2);
1606 
1607  r2_x_min = grid2->getXMin();
1608  r2_x_max = grid2->getXMax();
1609  r2_y_min = grid2->getYMin();
1610  r2_y_max = grid2->getYMax();
1611 
1612  if (r2_x_max - r2_x_min > 2 * margin_to_substract)
1613  {
1614  r2_x_max -= margin_to_substract;
1615  r2_x_min += margin_to_substract;
1616  }
1617  if (r2_y_max - r2_y_min > 2 * margin_to_substract)
1618  {
1619  r2_y_max -= margin_to_substract;
1620  r2_y_min += margin_to_substract;
1621  }
1622 
1623  // Compute the probability:
1624  for (i = 0; i < monteCarloSamples; i++)
1625  {
1627  r1_x_min, r1_x_max, r1_y_min, r1_y_max, r2_x_min, r2_x_max,
1628  r2_y_min, r2_y_max, posePDF.m_particles[i].d.x,
1629  posePDF.m_particles[i].d.y, posePDF.m_particles[i].d.yaw))
1630  {
1631  hits++;
1632  }
1633  }
1634 
1635  return static_cast<double>(hits) / monteCarloSamples;
1636 
1637  MRPT_END
1638 }
A namespace of pseudo-random numbers generators of diferent distributions.
bool RectanglesIntersection(const double &R1_x_min, const double &R1_x_max, const double &R1_y_min, const double &R1_y_max, const double &R2_x_min, const double &R2_x_max, const double &R2_y_min, const double &R2_y_max, const double &R2_pose_x, const double &R2_pose_y, const double &R2_pose_phi)
Returns whether two rotated rectangles intersect.
Definition: geometry.cpp:370
#define MRPT_START
Definition: exceptions.h:241
CHMHMapNode::TNodeID id
CPose3D mean
The mean value.
static Ptr Create(Args &&... args)
Definition: CText.h:37
void optimize_graph_spa_levmarq(GRAPH_T &graph, TResultInfoSpaLevMarq &out_info, const std::set< mrpt::graphs::TNodeID > *in_nodes_to_optimize=nullptr, const mrpt::system::TParametersDouble &extra_params=mrpt::system::TParametersDouble(), FEEDBACK_CALLABLE functor_feedback=FEEDBACK_CALLABLE())
Optimize a graph of pose constraints using the Sparse Pose Adjustment (SPA) sparse representation and...
Definition: levmarq.h:79
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
mrpt::graphs::TNodeID TNodeID
The type of the IDs of nodes.
Definition: CHMHMapNode.h:44
CParticleList m_particles
The array of particles.
double normalizeWeights(double *out_max_log_w=nullptr) override
Normalize the (logarithmic) weights, such as the maximum weight is zero.
mrpt::graphs::TNodeID root
The ID of the node that is the origin of coordinates, used as reference by all coordinates in nodes...
void dijkstra_nodes_estimate(std::optional< std::reference_wrapper< std::map< TNodeID, size_t >>> topological_distances=std::nullopt)
Spanning tree computation of a simple estimation of the global coordinates of each node just from the...
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
This file implements several operations that operate element-wise on individual or pairs of container...
Declares a class that represents a Probability Density function (PDF) of a 3D(6D) pose ...
Definition: CPose3DPDFSOG.h:32
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:552
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:546
STL namespace.
static Ptr Create(Args &&... args)
Definition: CSetOfObjects.h:28
#define AREAID_INVALID
GLdouble s
Definition: glext.h:3682
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
const TDistance & operator=(const unsigned &D)
A directed graph of pose constraints, with edges being the relative poses between pairs of nodes iden...
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
void getMean(CPose3D &mean_pose) const override
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF), computed as a weighted average over all m_particles.
This base provides a set of functions for maths stuff.
GLsizei samples
Definition: glext.h:8217
void addComponents(const CPose3D &p)
Scalar sum of all 6 components: This is diferent from poses composition, which is implemented as "+" ...
Definition: CPose3D.cpp:346
Auxiliary class used in mrpt::slam::CLocalMetricHypothesis for HMT-SLAM; this class stores a set of r...
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
void clear(bool createMainViewport=true)
Clear the list of objects and viewports in the scene, deleting objects&#39; memory, and leaving just the ...
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
void drawManySamples(size_t N, std::vector< mrpt::math::CVectorDouble > &outSamples) const override
Draws a number of samples from the distribution, and saves as a list of 1x6 vectors, where each row contains a (x,y,phi) datum.
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
#define NODE_ANNOTATION_METRIC_MAPS
GLsizei const GLchar ** string
Definition: glext.h:4116
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:558
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
A class for storing an occupancy grid map.
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define ARC_ANNOTATION_DELTA_SRC_POSEID
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
TDistance(const unsigned &D)
GLuint id
Definition: glext.h:3920
#define MRPT_END
Definition: exceptions.h:245
Output information for mrpt::graphslam::optimize_graph_spa_levmarq()
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
Definition: COpenGLScene.h:58
GLsizei GLsizei GLchar * source
Definition: glext.h:4097
static Ptr Create(Args &&... args)
Definition: CSphere.h:30
#define NODE_ANNOTATION_REF_POSEID
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
void normalizeAngles()
Rebuild the internal matrix & update the yaw/pitch/roll angles within the ]-PI,PI] range (Must be cal...
Definition: CPose3D.cpp:261
global_poses_t nodes
The nodes (vertices) of the graph, with their estimated "global" (with respect to root) position...
GLenum GLsizei GLenum format
Definition: glext.h:3535
void resetDeterministic(const mrpt::math::TPose3D &location, size_t particlesCount=0)
Reset the PDF to a single point: All m_particles will be set exactly to the supplied pose...
GLsizeiptr size
Definition: glext.h:3934
static Ptr Create(Args &&... args)
Definition: CDisk.h:32
void insertEdgeAtEnd(TNodeID from_nodeID, TNodeID to_nodeID, const edge_t &edge_value)
Insert an edge (from -> to) with the given edge value (more efficient version to be called if you kno...
This class stores any customizable set of metric maps.
#define NODE_ANNOTATION_POSES_GRAPH
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
void insert(const CRenderizable::Ptr &newObject, const std::string &viewportName=std::string("main"))
Insert a new object into the scene, in the given viewport (by default, into the "main" viewport)...
#define ARC_ANNOTATION_DELTA_TRG_POSEID
GLubyte GLubyte GLubyte a
Definition: glext.h:6372
GLfloat GLfloat p
Definition: glext.h:6398
static Ptr Create(Args &&... args)
Definition: CGridPlaneXY.h:31
A class for storing a sequence of arcs (a path).
static Ptr Create(Args &&... args)
Definition: CSimpleLine.h:21
#define ARC_ANNOTATION_DELTA
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
int _strcmpi(const char *str1, const char *str2) noexcept
An OS-independent version of strcmpi.
Definition: os.cpp:322



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 9b18308f3 Mon Nov 18 23:39:25 2019 +0100 at lun nov 18 23:45:12 CET 2019