MRPT  2.0.3
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 }
os.h
mrpt::poses::CPose3DPDFSOG
Declares a class that represents a Probability Density function (PDF) of a 3D(6D) pose .
Definition: CPose3DPDFSOG.h:32
CRobotPosesGraph.h
ops_containers.h
mrpt::graphs::CDirectedGraph::insertEdgeAtEnd
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...
Definition: CDirectedGraph.h:134
mrpt::graphs::CNetworkOfPoses::nodes
global_poses_t nodes
The nodes (vertices) of the graph, with their estimated "global" (with respect to root) position,...
Definition: CNetworkOfPoses.h:232
mrpt::poses::CPose3DPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
Definition: CPose3DPDFGaussian.h:39
mrpt::hmtslam::CHMHMapNode::Ptr
std::shared_ptr< mrpt::hmtslam ::CHMHMapNode > Ptr
Definition: CHMHMapNode.h:39
TDistance::operator=
const TDistance & operator=(const unsigned &D)
Definition: CHierarchicalMapMHPartition.cpp:708
mrpt::opengl::CSetOfObjects::Create
static Ptr Create(Args &&... args)
Definition: CSetOfObjects.h:28
mrpt::poses::CPose3DPDFParticles::resetDeterministic
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.
Definition: CPose3DPDFParticles.cpp:311
mrpt::serialization::CSerializable::Ptr
std::shared_ptr< CSerializable > Ptr
Definition: CSerializable.h:36
mrpt::maps::CMultiMetricMap::mapByClass
T::Ptr mapByClass(size_t ith=0) const
Returns the i'th map of a given class (or of a derived class), or empty smart pointer if there is no ...
Definition: CMultiMetricMap.h:156
mrpt::opengl::CSetOfObjects::Ptr
std::shared_ptr< mrpt::opengl ::CSetOfObjects > Ptr
Definition: CSetOfObjects.h:28
mrpt::opengl::COpenGLScene::insert
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).
Definition: COpenGLScene.cpp:177
mrpt::poses::CPose3D::pitch
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:552
mrpt::opengl::CGridPlaneXY::Ptr
std::shared_ptr< mrpt::opengl ::CGridPlaneXY > Ptr
Definition: CGridPlaneXY.h:31
aligned_allocator.h
CSimpleLine.h
CSphere.h
mrpt::poses::CPose3D::addComponents
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
mrpt::graphslam::optimize_graph_spa_levmarq
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
TPrevious::id
CHMHMapNode::TNodeID id
Definition: CHierarchicalMapMHPartition.cpp:720
CSetOfObjects.h
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
mrpt::opengl::COpenGLScene
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives.
Definition: COpenGLScene.h:56
NODE_ANNOTATION_REF_POSEID
#define NODE_ANNOTATION_REF_POSEID
Definition: HMT_SLAM_common.h:18
ARC_ANNOTATION_DELTA_SRC_POSEID
#define ARC_ANNOTATION_DELTA_SRC_POSEID
Definition: HMT_SLAM_common.h:26
mrpt::graphslam::TResultInfoSpaLevMarq
Output information for mrpt::graphslam::optimize_graph_spa_levmarq()
Definition: graphslam/include/mrpt/graphslam/types.h:68
mrpt::math::MatrixBase::setIdentity
void setIdentity()
Definition: MatrixBase.h:57
mrpt::math::RectanglesIntersection
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
mrpt::maps::CMultiMetricMap::Ptr
std::shared_ptr< mrpt::maps ::CMultiMetricMap > Ptr
Definition: CMultiMetricMap.h:122
mrpt::opengl::CDisk::Ptr
std::shared_ptr< mrpt::opengl ::CDisk > Ptr
Definition: CDisk.h:32
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
RAD2DEG
#define RAD2DEG
Definition: core/include/mrpt/core/bits_math.h:80
NODE_ANNOTATION_POSES_GRAPH
#define NODE_ANNOTATION_POSES_GRAPH
Definition: HMT_SLAM_common.h:19
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
hmtslam-precomp.h
ARC_ANNOTATION_DELTA_TRG_POSEID
#define ARC_ANNOTATION_DELTA_TRG_POSEID
Definition: HMT_SLAM_common.h:28
random.h
mrpt::poses::CPose3DPDFGaussian::drawManySamples
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,...
Definition: CPose3DPDFGaussian.cpp:342
mrpt::hmtslam::CRobotPosesGraph
Auxiliary class used in mrpt::slam::CLocalMetricHypothesis for HMT-SLAM; this class stores a set of r...
Definition: CRobotPosesGraph.h:36
mrpt::hmtslam
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
Definition: CHierarchicalMapMHPartition.h:27
ARC_ANNOTATION_DELTA
#define ARC_ANNOTATION_DELTA
Definition: HMT_SLAM_common.h:23
mrpt::opengl::CGridPlaneXY::Create
static Ptr Create(Args &&... args)
Definition: CGridPlaneXY.h:31
CDisk.h
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
mrpt::poses::CPose3DPDFGaussianInf::copyFrom
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations)
Definition: CPose3DPDFGaussianInf.cpp:105
mrpt::system::TParameters< double >
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:39
mrpt::maps::CMultiMetricMap
This class stores any customizable set of metric maps.
Definition: CMultiMetricMap.h:120
mrpt::hmtslam::CHMHMapNode::TNodeID
mrpt::graphs::TNodeID TNodeID
The type of the IDs of nodes.
Definition: CHMHMapNode.h:44
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::poses::CPose3DPDFGaussianInf
Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...
Definition: CPose3DPDFGaussianInf.h:39
mrpt::opengl::CDisk::Create
static Ptr Create(Args &&... args)
Definition: CDisk.h:32
mrpt::poses::CPoseOrPoint::y
double y() const
Definition: CPoseOrPoint.h:147
mrpt::poses::CPose3D::roll
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:558
mrpt::math::size
size_t size(const MATRIXLIKE &m, const int dim)
Definition: math/include/mrpt/math/bits_math.h:21
TPrevious
Definition: CHierarchicalMapMHPartition.cpp:717
mrpt::math::TPose3D
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
levmarq.h
mrpt::poses::CPose3DPDFParticles
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
Definition: CPose3DPDFParticles.h:29
mrpt::serialization
Definition: aligned_serialization.h:13
TPrevious::TPrevious
TPrevious()
Definition: CHierarchicalMapMHPartition.cpp:719
ASSERTMSG_
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
mrpt::opengl::CSphere::Ptr
std::shared_ptr< mrpt::opengl ::CSphere > Ptr
Definition: CSphere.h:31
mrpt::opengl::CText::Create
static Ptr Create(Args &&... args)
Definition: CText.h:37
mrpt::hmtslam::CRobotPosesGraph::Ptr
std::shared_ptr< mrpt::hmtslam ::CRobotPosesGraph > Ptr
Definition: CRobotPosesGraph.h:39
mrpt::bayes::CParticleFilterData::m_particles
CParticleList m_particles
The array of particles.
Definition: CParticleFilterData.h:195
mrpt::hmtslam::TPoseID
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
Definition: HMT_SLAM_common.h:63
AREAID_INVALID
#define AREAID_INVALID
Definition: HMT_SLAM_common.h:42
mrpt::hmtslam::TArcList
A class for storing a sequence of arcs (a path).
Definition: HMT_SLAM_common.h:103
mrpt::hmtslam::THypothesisID
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
Definition: HMT_SLAM_common.h:58
mrpt::graphs::CNetworkOfPoses::dijkstra_nodes_estimate
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...
Definition: CNetworkOfPoses.h:355
mrpt::graphs::CNetworkOfPoses
A directed graph of pose constraints, with edges being the relative poses between pairs of nodes iden...
Definition: CNetworkOfPoses.h:121
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:89
TDistance::TDistance
TDistance(const unsigned &D)
Definition: CHierarchicalMapMHPartition.cpp:707
TDistance
Definition: CHierarchicalMapMHPartition.cpp:704
mrpt::opengl::COpenGLScene::clear
void clear(bool createMainViewport=true)
Clear the list of objects and viewports in the scene, deleting objects' memory, and leaving just the ...
Definition: COpenGLScene.cpp:60
mrpt::poses::CPose3D::yaw
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:546
mrpt::poses::CPose3D::normalizeAngles
void normalizeAngles()
Rebuild the internal matrix & update the yaw/pitch/roll angles within the ]-PI,PI] range (Must be cal...
Definition: CPose3D.cpp:261
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::graphs::CNetworkOfPoses::root
mrpt::graphs::TNodeID root
The ID of the node that is the origin of coordinates, used as reference by all coordinates in nodes.
Definition: CNetworkOfPoses.h:237
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::hmtslam::CHMHMapArc::Ptr
std::shared_ptr< mrpt::hmtslam ::CHMHMapArc > Ptr
Definition: CHMHMapArc.h:35
mrpt::maps::COccupancyGridMap2D
A class for storing an occupancy grid map.
Definition: COccupancyGridMap2D.h:53
mrpt::poses::CPose3DPDFGaussian::cov
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
Definition: CPose3DPDFGaussian.h:82
mrpt::poses::CPose3DPDFParticles::getMean
void getMean(CPose3D &mean_pose) const override
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF),...
Definition: CPose3DPDFParticles.cpp:52
mrpt::opengl::CSphere::Create
static Ptr Create(Args &&... args)
Definition: CSphere.h:31
mrpt::random
A namespace of pseudo-random numbers generators of diferent distributions.
Definition: random_shuffle.h:18
mrpt::maps
Definition: CBeacon.h:21
mrpt::poses::CPose3DPDFGaussian::copyFrom
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations)
Definition: CPose3DPDFGaussian.cpp:245
mrpt::system::os::_strcmpi
int _strcmpi(const char *str1, const char *str2) noexcept
An OS-independent version of strcmpi.
Definition: os.cpp:320
mrpt::opengl::CText::Ptr
std::shared_ptr< mrpt::opengl ::CText > Ptr
Definition: CText.h:37
CGridPlaneXY.h
mrpt::slam
Definition: CMultiMetricMapPDF.h:26
mrpt::opengl
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
TDistance::TDistance
TDistance()
Definition: CHierarchicalMapMHPartition.cpp:706
mrpt::poses::CPose3DPDFGaussian::mean
CPose3D mean
The mean value.
Definition: CPose3DPDFGaussian.h:78
mrpt::opengl::CSimpleLine::Create
static Ptr Create(Args &&... args)
Definition: CSimpleLine.h:21
mrpt::format
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
TDistance::dist
unsigned dist
Definition: CHierarchicalMapMHPartition.cpp:714
mrpt::system
Definition: backtrace.h:14
CText.h
mrpt::bayes::CParticleFilterDataImpl::normalizeWeights
double normalizeWeights(double *out_max_log_w=nullptr) override
Normalize the (logarithmic) weights, such as the maximum weight is zero.
Definition: CParticleFilterData.h:59
CPose3DPDFParticles.h
mrpt::opengl::CSimpleLine::Ptr
std::shared_ptr< mrpt::opengl ::CSimpleLine > Ptr
Definition: CSimpleLine.h:21
NODE_ANNOTATION_METRIC_MAPS
#define NODE_ANNOTATION_METRIC_MAPS
Definition: HMT_SLAM_common.h:17



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 29 13:06:46 UTC 2020