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


Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at mié 12 jul 2023 10:03:34 CEST