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



Page generated by Doxygen 1.8.14 for MRPT 2.0.5 Git: 40e60e732 Thu Jul 9 08:38:35 2020 +0200 at jue jul 9 08:45:11 CEST 2020