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-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "hmtslam-precomp.h" // Precomp header
11 
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::utils;
31 using namespace mrpt::system;
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  static const CPose3D nullPose(0, 0, 0);
876  CHMHMapNode::TNodeID lastNode, nextNode;
877  size_t pathLength;
878 
879  typedef mrpt::aligned_containers<CPose3D>::vector_t TPose3DList;
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 = nullPose - *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 = *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.isType(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 (
1093  (*itArc)->m_annotations.get(
1094  requiredAnnotation, hypothesisID))
1095  return true;
1096  }
1097  }
1098 
1099  return false;
1100 
1101  MRPT_END
1102 }
1103 
1104 /*---------------------------------------------------------------
1105  getAs3DScene
1106  ---------------------------------------------------------------*/
1107 
1108 void CHierarchicalMapMHPartition::getAs3DScene(
1109  COpenGLScene& outScene, const CHMHMapNode::TNodeID& idReferenceNode,
1110  const THypothesisID& hypothesisID,
1111  const unsigned int& numberOfIterationsForOptimalGlobalPoses,
1112  const bool& showRobotPoseIDs) const
1113 {
1114  MRPT_START
1115 
1116  // First: Clear and add the cool "ground grid" :-P
1117  outScene.clear();
1118  {
1119  mrpt::opengl::CGridPlaneXY::Ptr obj =
1120  mrpt::make_aligned_shared<mrpt::opengl::CGridPlaneXY>(
1121  -500, 500, -500, 500, 0, 5);
1122  obj->setColor(0.3, 0.3, 0.3);
1123  outScene.insert(obj);
1124  }
1125 
1126  typedef std::map<CHMHMapNode::TNodeID, CPose3DPDFGaussian,
1127  std::less<CHMHMapNode::TNodeID>,
1128  Eigen::aligned_allocator<std::pair<
1129  const CHMHMapNode::TNodeID, CPose3DPDFGaussian>>>
1130  TMapID2PosePDF;
1131  TMapID2PosePDF nodesPoses; // The ref. pose of each area
1132  TMapID2PosePDF::iterator it;
1133 
1134  typedef std::map<CHMHMapNode::TNodeID, CPose2D,
1135  std::less<CHMHMapNode::TNodeID>,
1136  Eigen::aligned_allocator<
1137  std::pair<const CHMHMapNode::TNodeID, CPose2D>>>
1138  TMapID2Pose2D;
1139  TMapID2Pose2D
1140  nodesMeanPoses; // The mean pose of the observations in the area
1141  TMapID2Pose2D::iterator it2;
1142 
1143  // Only those nodes in the "hypothesisID" are computed.
1144  computeGloballyConsistentNodeCoordinates(
1145  nodesPoses, idReferenceNode, hypothesisID,
1146  numberOfIterationsForOptimalGlobalPoses);
1147 
1148  // Find the mean of the observations in each area:
1149  for (it = nodesPoses.begin(); it != nodesPoses.end(); it++)
1150  {
1151  CPose2D meanPose = CPose2D(it->second.mean);
1152  const CHMHMapNode::Ptr node = getNodeByID(it->first);
1153 
1154  CRobotPosesGraph::Ptr posesGraph =
1155  node->m_annotations.getAs<CRobotPosesGraph>(
1156  NODE_ANNOTATION_POSES_GRAPH, hypothesisID);
1157 
1158  if (posesGraph && posesGraph->size())
1159  {
1160  // Compute the mean pose:
1161  CPose3D meanSFs(0, 0, 0);
1162 
1163  for (CRobotPosesGraph::const_iterator it = posesGraph->begin();
1164  it != posesGraph->end(); ++it)
1165  meanSFs.addComponents(it->second.pdf.getMeanVal());
1166 
1167  meanSFs *= 1.0f / (posesGraph->size());
1168  meanSFs.normalizeAngles();
1169 
1170  meanPose = meanPose + CPose2D(meanSFs);
1171  }
1172  else
1173  {
1174  // Let the ref. pose to represent the node
1175  }
1176 
1177  nodesMeanPoses[it->first] = meanPose;
1178  }
1179 
1180  // ----------------------------------------------------------------
1181  // Now we have the global poses of all the m_nodes: Draw'em all
1182  // ---------------------------------------------------------------
1183  // Draw the (grid maps) metric maps of each area:
1184  for (it = nodesPoses.begin(); it != nodesPoses.end(); it++)
1185  {
1186  const CPose3D& pose = it->second.mean;
1187  const CHMHMapNode::Ptr node = getNodeByID(it->first);
1188 
1189  CMultiMetricMap::Ptr metricMap =
1190  node->m_annotations.getAs<CMultiMetricMap>(
1191  NODE_ANNOTATION_METRIC_MAPS, hypothesisID);
1192  if (metricMap) // ASSERT_(metricMap);
1193  {
1194  mrpt::opengl::CSetOfObjects::Ptr objTex =
1195  mrpt::make_aligned_shared<mrpt::opengl::CSetOfObjects>();
1196  metricMap->getAs3DObject(objTex);
1197  objTex->setPose(pose);
1198  outScene.insert(objTex);
1199  }
1200  }
1201 
1202  float nodes_height = 10.0f;
1203  float nodes_radius = 1.0f;
1204 
1205  // Draw the m_nodes:
1206  // ----------------------
1207  for (it = nodesPoses.begin(), it2 = nodesMeanPoses.begin();
1208  it != nodesPoses.end(); it++, it2++)
1209  {
1210  const CHMHMapNode::Ptr node = getNodeByID(it->first);
1211  const CPose3D& pose = it->second.mean;
1212  const CPose3D meanPose = CPose3D(it2->second);
1213 
1214  // The sphere of the node:
1215  mrpt::opengl::CSphere::Ptr objSphere =
1216  mrpt::make_aligned_shared<mrpt::opengl::CSphere>();
1217 
1218  objSphere->setName(node->m_label);
1219  objSphere->setColor(0, 0, 1);
1220  objSphere->setLocation(
1221  meanPose.x(), meanPose.y(), meanPose.z() + nodes_height);
1222  objSphere->setRadius(nodes_radius);
1223  objSphere->setNumberDivsLongitude(16);
1224  objSphere->setNumberDivsLatitude(16);
1225 
1226  objSphere->enableRadiusIndependentOfEyeDistance();
1227 
1228  outScene.insert(objSphere);
1229 
1230  // The label with the name of the node:
1231  mrpt::opengl::CText::Ptr objText =
1232  mrpt::make_aligned_shared<mrpt::opengl::CText>();
1233  // objText->m_str = node->m_label;
1234  objText->setString(format("%li", (long int)node->getID()));
1235  // objText->m_fontHeight = 20;
1236  objText->setColor(1, 0, 0);
1237  objText->setLocation(
1238  meanPose.x(), meanPose.y(), meanPose.z() + nodes_height);
1239 
1240  outScene.insert(objText);
1241 
1242  // And the observations "on the ground" as disks
1243  // ---------------------------------------------------
1244  CRobotPosesGraph::Ptr posesGraph =
1245  node->m_annotations.getAs<CRobotPosesGraph>(
1246  NODE_ANNOTATION_POSES_GRAPH, hypothesisID);
1247  // ASSERT_(posesGraph);
1248 
1249  if (posesGraph)
1250  {
1251  for (CRobotPosesGraph::const_iterator it = posesGraph->begin();
1252  it != posesGraph->end(); ++it)
1253  {
1254  CPose3D SF_pose;
1255  it->second.pdf.getMean(SF_pose);
1256 
1257  CPose3D auxPose(pose + SF_pose);
1258 
1259  mrpt::opengl::CDisk::Ptr glObj =
1260  mrpt::make_aligned_shared<mrpt::opengl::CDisk>();
1261 
1262  glObj->setColor(1, 0, 0);
1263 
1264  glObj->setPose(auxPose);
1265  // glObj->m_z = 0;
1266 
1267  glObj->setDiskRadius(0.05f);
1268  glObj->setSlicesCount(20);
1269  glObj->setLoopsCount(10);
1270 
1271  if (showRobotPoseIDs)
1272  {
1273  glObj->setName(format("%i", (int)it->first));
1274  glObj->enableShowName();
1275  }
1276 
1277  outScene.insert(glObj);
1278 
1279  // And a line up-to the node:
1280  mrpt::opengl::CSimpleLine::Ptr objLine =
1281  mrpt::make_aligned_shared<mrpt::opengl::CSimpleLine>();
1282 
1283  objLine->setColor(1, 0, 0, 0.2);
1284  objLine->setLineWidth(1.5);
1285 
1286  objLine->setLineCoords(
1287  meanPose.x(), meanPose.y(), nodes_height, auxPose.x(),
1288  auxPose.y(), 0);
1289 
1290  outScene.insert(objLine);
1291  } // end for observation "disks"
1292  }
1293 
1294  // And their m_arcs:
1295  // ----------------------
1296  TArcList arcs;
1297  node->getArcs(arcs, hypothesisID);
1298  for (TArcList::const_iterator a = arcs.begin(); a != arcs.end(); ++a)
1299  {
1300  CHMHMapArc::Ptr arc = *a;
1301 
1302  if (arc->getNodeFrom() == node->getID())
1303  {
1304  CPose3D poseTo(nodesMeanPoses.find(arc->getNodeTo())->second);
1305 
1306  // Draw the line:
1307  mrpt::opengl::CSimpleLine::Ptr objLine =
1308  mrpt::make_aligned_shared<mrpt::opengl::CSimpleLine>();
1309 
1310  objLine->setColor(0, 1, 0, 0.5);
1311  objLine->setLineWidth(5);
1312 
1313  objLine->setLineCoords(
1314  meanPose.x(), meanPose.y(), meanPose.z() + nodes_height,
1315  poseTo.x(), poseTo.y(), poseTo.z() + nodes_height);
1316 
1317  outScene.insert(objLine);
1318  }
1319  }
1320  }
1321 
1322  MRPT_END
1323 }
1324 
1325 /*---------------------------------------------------------------
1326  computeGloballyConsistentNodeCoordinates
1327  ---------------------------------------------------------------*/
1328 void CHierarchicalMapMHPartition::computeGloballyConsistentNodeCoordinates(
1329  std::map<CHMHMapNode::TNodeID, CPose3DPDFGaussian,
1330  std::less<CHMHMapNode::TNodeID>,
1331  Eigen::aligned_allocator<std::pair<
1332  const CHMHMapNode::TNodeID, CPose3DPDFGaussian>>>& nodePoses,
1333  const CHMHMapNode::TNodeID& idReferenceNode,
1334  const THypothesisID& hypothesisID,
1335  const unsigned int& numberOfIterations) const
1336 {
1337  MRPT_START
1338 
1339  nodePoses.clear();
1340 
1341  if (m_arcs.empty()) return; // Nothing we can do!
1342 
1343  // 1) Convert hmt-slam graph into graphslam graph... (this should be avoided
1344  // in future version of HTML-SLAM!!)
1345  graphs::CNetworkOfPoses3DInf pose_graph;
1346 
1347  for (TArcList::const_iterator it_arc = m_arcs.begin();
1348  it_arc != m_arcs.end(); ++it_arc)
1349  {
1350  if (!(*it_arc)->m_hypotheses.has(hypothesisID)) continue;
1351 
1352  const CHMHMapNode::TNodeID id_from = (*it_arc)->getNodeFrom();
1353  const CHMHMapNode::TNodeID id_to = (*it_arc)->getNodeTo();
1354 
1355  CSerializable::Ptr anotation =
1356  (*it_arc)->m_annotations.get(ARC_ANNOTATION_DELTA, hypothesisID);
1357  if (!anotation) continue;
1358 
1359  CPose3DPDFGaussianInf edge_rel_pose_pdf; // Convert to gaussian
1360  edge_rel_pose_pdf.copyFrom(
1361  *std::dynamic_pointer_cast<CPose3DPDF>(anotation));
1362 
1363  pose_graph.insertEdgeAtEnd(id_from, id_to, edge_rel_pose_pdf);
1364  }
1365 
1366  // 2) Initialize global poses of nodes with Dijkstra:
1367  pose_graph.root = idReferenceNode;
1368  pose_graph.dijkstra_nodes_estimate();
1369 
1370  // 3) Optimize with graph-slam:
1371  graphslam::TResultInfoSpaLevMarq out_info;
1372  TParametersDouble graphslam_params;
1373  graphslam_params["max_iterations"] = numberOfIterations;
1374 
1375  graphslam::optimize_graph_spa_levmarq(
1376  pose_graph, out_info,
1377  nullptr, // Optimize all nodes
1378  graphslam_params);
1379 
1380  // 4) Copy back optimized results into the HMT-SLAM graph:
1381  for (graphs::CNetworkOfPoses3DInf::global_poses_t::const_iterator it_node =
1382  pose_graph.nodes.begin();
1383  it_node != pose_graph.nodes.end(); ++it_node)
1384  {
1385  const CHMHMapNode::TNodeID node_id = it_node->first;
1386 
1387  // To the output map:
1388  CPose3DPDFGaussian& new_pose = nodePoses[node_id];
1389  new_pose.mean = it_node->second;
1390  new_pose.cov.setIdentity(); // *** At present, graphslam does not
1391  // output the uncertainty of poses... ***
1392  }
1393 
1394 #if __computeGloballyConsistentNodeCoordinates__VERBOSE
1395  for (map<CHMHMapNode::TNodeID, CPose3DPDFGaussian>::const_iterator it =
1396  nodePoses.begin();
1397  it != nodePoses.end(); ++it)
1398  cout << it->first << ": " << it->second.mean << endl;
1399  cout << endl;
1400 #endif
1401 
1402  MRPT_END
1403 }
1404 
1405 /*---------------------------------------------------------------
1406  dumpAsText
1407  ---------------------------------------------------------------*/
1408 void CHierarchicalMapMHPartition::dumpAsText(utils::CStringList& st) const
1409 {
1410  st.clear();
1411  st << "LIST OF NODES";
1412  st << "================";
1413 
1414  for (TNodeList::const_iterator it = m_nodes.begin(); it != m_nodes.end();
1415  ++it)
1416  {
1417  std::string s;
1418  s += format(
1419  "NODE ID: %i\t LABEL:%s\tARCS: ", (int)it->second->getID(),
1420  it->second->m_label.c_str());
1421  TArcList arcs;
1422  it->second->getArcs(arcs);
1423  for (TArcList::const_iterator a = arcs.begin(); a != arcs.end(); ++a)
1424  s += format(
1425  "%i-%i, ", (int)(*a)->getNodeFrom(), (int)(*a)->getNodeTo());
1426 
1427  st << s;
1428 
1429  for (CMHPropertiesValuesList::const_iterator ann =
1430  it->second->m_annotations.begin();
1431  ann != it->second->m_annotations.end(); ++ann)
1432  {
1433  s = format(
1434  " [HYPO ID #%02i] Annotation '%s' Class: ", (int)ann->ID,
1435  ann->name.c_str());
1436  if (ann->value)
1437  s += string(ann->value->GetRuntimeClass()->className);
1438  else
1439  s += "(nullptr)";
1440 
1441  st << s;
1442 
1443  if (ann->name == NODE_ANNOTATION_REF_POSEID)
1444  {
1445  TPoseID refID;
1446  it->second->m_annotations.getElemental(
1447  NODE_ANNOTATION_REF_POSEID, refID, ann->ID);
1448  st << format(" VALUE: %i", (int)refID);
1449  }
1450  else if (ann->name == NODE_ANNOTATION_POSES_GRAPH)
1451  {
1452  CRobotPosesGraph::Ptr posesGraph =
1453  it->second->m_annotations.getAs<CRobotPosesGraph>(
1454  NODE_ANNOTATION_POSES_GRAPH, ann->ID);
1455  ASSERT_(posesGraph);
1456 
1457  st << format(
1458  " CRobotPosesGraph has %i poses:",
1459  (int)posesGraph->size());
1460  CPose3D pdfMean;
1461  for (CRobotPosesGraph::const_iterator p = posesGraph->begin();
1462  p != posesGraph->end(); ++p)
1463  {
1464  const CPose3DPDFParticles& pdf = p->second.pdf;
1465  pdf.getMean(pdfMean);
1466  st << format(
1467  " Pose %i \t (%.03f,%.03f,%.03fdeg)",
1468  (int)p->first, pdfMean.x(), pdfMean.y(),
1469  RAD2DEG(pdfMean.yaw()));
1470  }
1471  }
1472  }
1473 
1474  st << "";
1475  }
1476 
1477  st << "";
1478  st << "";
1479  st << "LIST OF ARCS";
1480  st << "================";
1481 
1482  for (TArcList::const_iterator it = m_arcs.begin(); it != m_arcs.end(); ++it)
1483  {
1484  std::string s;
1485  s += format(
1486  "ARC: %i -> %i\n", (int)(*it)->getNodeFrom(),
1487  (int)(*it)->getNodeTo());
1488 
1489  s += string(" Arc type: ") + (*it)->m_arcType.getType();
1490 
1491  st << s;
1492 
1493  for (CMHPropertiesValuesList::const_iterator ann =
1494  (*it)->m_annotations.begin();
1495  ann != (*it)->m_annotations.end(); ++ann)
1496  {
1497  s = format(
1498  " [HYPO ID #%02i] Annotation '%s' Class: ", (int)ann->ID,
1499  ann->name.c_str());
1500  if (ann->value)
1501  s += string(ann->value->GetRuntimeClass()->className);
1502  else
1503  s += "(nullptr)";
1504 
1505  st << s;
1506 
1507  if (ann->name == ARC_ANNOTATION_DELTA_SRC_POSEID)
1508  {
1509  TPoseID refID;
1510  (*it)->m_annotations.getElemental(
1511  ARC_ANNOTATION_DELTA_SRC_POSEID, refID, ann->ID);
1512  st << format(" VALUE: %i", (int)refID);
1513  }
1514  else if (ann->name == ARC_ANNOTATION_DELTA_TRG_POSEID)
1515  {
1516  TPoseID refID;
1517  (*it)->m_annotations.getElemental(
1518  ARC_ANNOTATION_DELTA_TRG_POSEID, refID, ann->ID);
1519  st << format(" VALUE: %i", (int)refID);
1520  }
1521  else if (ann->name == ARC_ANNOTATION_DELTA)
1522  {
1523  CSerializable::Ptr o =
1524  (*it)->m_annotations.get(ARC_ANNOTATION_DELTA, ann->ID);
1525  ASSERT_(o);
1526 
1527  CPose3DPDFGaussian relativePoseAcordToArc;
1528  relativePoseAcordToArc.copyFrom(
1529  *std::dynamic_pointer_cast<CPose3DPDF>(o));
1530 
1531  st << format(
1532  " VALUE: (%f,%f,%f , %fdeg,%fdeg,%fdeg)",
1533  relativePoseAcordToArc.mean.x(),
1534  relativePoseAcordToArc.mean.y(),
1535  relativePoseAcordToArc.mean.z(),
1536  RAD2DEG(relativePoseAcordToArc.mean.yaw()),
1537  RAD2DEG(relativePoseAcordToArc.mean.pitch()),
1538  RAD2DEG(relativePoseAcordToArc.mean.roll()));
1539  }
1540  }
1541 
1542  st << "";
1543  }
1544 }
1545 
1546 /*---------------------------------------------------------------
1547  findArcOfTypeBetweenNodes
1548  ---------------------------------------------------------------*/
1549 CHMHMapArc::Ptr CHierarchicalMapMHPartition::findArcOfTypeBetweenNodes(
1550  const CHMHMapNode::TNodeID& node1id, const CHMHMapNode::TNodeID& node2id,
1551  const THypothesisID& hypothesisID, const std::string& arcType,
1552  bool& isInverted) const
1553 {
1554  MRPT_START
1555 
1556  TArcList lstArcs;
1557  findArcsOfTypeBetweenNodes(
1558  node1id, node2id, hypothesisID, arcType, lstArcs);
1559 
1560  for (TArcList::const_iterator a = lstArcs.begin(); a != lstArcs.end(); ++a)
1561  {
1562  if ((*a)->getNodeFrom() == node1id)
1563  {
1564  // Found, and in the correct direction:
1565  isInverted = false;
1566  return *a;
1567  }
1568  else
1569  {
1570  // Found, in the opossite direction:
1571  isInverted = true;
1572  return *a;
1573  }
1574  }
1575 
1576  return CHMHMapArc::Ptr();
1577  MRPT_END
1578 }
1579 
1580 /*---------------------------------------------------------------
1581  computeOverlapProbabilityBetweenNodes
1582  ---------------------------------------------------------------*/
1583 double CHierarchicalMapMHPartition::computeOverlapProbabilityBetweenNodes(
1584  const CHMHMapNode::TNodeID& nodeFrom, const CHMHMapNode::TNodeID& nodeTo,
1585  const THypothesisID& hypothesisID, const size_t& monteCarloSamples,
1586  const float margin_to_substract) const
1587 {
1588  MRPT_START
1589 
1590  size_t i, hits = 0;
1591  CPose3DPDFParticles posePDF;
1592  const CHMHMapNode::Ptr from = getNodeByID(nodeFrom);
1593  const CHMHMapNode::Ptr to = getNodeByID(nodeTo);
1594 
1595  // Draw pose samples:
1596  computeCoordinatesTransformationBetweenNodes(
1597  nodeFrom, nodeTo, posePDF, hypothesisID, monteCarloSamples);
1598  /*0.15f,
1599  DEG2RAD(3.0f) );*/
1600 
1601  // Get the extends of grid maps:
1602  float r1_x_min, r1_x_max, r1_y_min, r1_y_max, r2_x_min, r2_x_max, r2_y_min,
1603  r2_y_max;
1604 
1605  // MAP1:
1606  CMultiMetricMap::Ptr hMap1 = from->m_annotations.getAs<CMultiMetricMap>(
1607  NODE_ANNOTATION_METRIC_MAPS, hypothesisID, false);
1608 
1609  ASSERT_(hMap1->m_gridMaps.size() > 0);
1610 
1611  r1_x_min = hMap1->m_gridMaps[0]->getXMin();
1612  r1_x_max = hMap1->m_gridMaps[0]->getXMax();
1613  r1_y_min = hMap1->m_gridMaps[0]->getYMin();
1614  r1_y_max = hMap1->m_gridMaps[0]->getYMax();
1615 
1616  if (r1_x_max - r1_x_min > 2 * margin_to_substract)
1617  {
1618  r1_x_max -= margin_to_substract;
1619  r1_x_min += margin_to_substract;
1620  }
1621  if (r1_y_max - r1_y_min > 2 * margin_to_substract)
1622  {
1623  r1_y_max -= margin_to_substract;
1624  r1_y_min += margin_to_substract;
1625  }
1626 
1627  // MAP2:
1628  CMultiMetricMap::Ptr hMap2 = to->m_annotations.getAs<CMultiMetricMap>(
1629  NODE_ANNOTATION_METRIC_MAPS, hypothesisID, false);
1630 
1631  ASSERT_(hMap2->m_gridMaps.size() > 0);
1632 
1633  r2_x_min = hMap2->m_gridMaps[0]->getXMin();
1634  r2_x_max = hMap2->m_gridMaps[0]->getXMax();
1635  r2_y_min = hMap2->m_gridMaps[0]->getYMin();
1636  r2_y_max = hMap2->m_gridMaps[0]->getYMax();
1637 
1638  if (r2_x_max - r2_x_min > 2 * margin_to_substract)
1639  {
1640  r2_x_max -= margin_to_substract;
1641  r2_x_min += margin_to_substract;
1642  }
1643  if (r2_y_max - r2_y_min > 2 * margin_to_substract)
1644  {
1645  r2_y_max -= margin_to_substract;
1646  r2_y_min += margin_to_substract;
1647  }
1648 
1649  // Compute the probability:
1650  for (i = 0; i < monteCarloSamples; i++)
1651  {
1652  if (math::RectanglesIntersection(
1653  r1_x_min, r1_x_max, r1_y_min, r1_y_max, r2_x_min, r2_x_max,
1654  r2_y_min, r2_y_max, posePDF.m_particles[i].d->x(),
1655  posePDF.m_particles[i].d->y(), posePDF.m_particles[i].d->yaw()))
1656  {
1657  hits++;
1658  }
1659  }
1660 
1661  return static_cast<double>(hits) / monteCarloSamples;
1662 
1663  MRPT_END
1664 }
mrpt::hmtslam::CRobotPosesGraph::Ptr
std::shared_ptr< CRobotPosesGraph > Ptr
Definition: CRobotPosesGraph.h:42
mrpt::random
A namespace of pseudo-random numbers genrators of diferent distributions.
Definition: RandomGenerators.h:25
mrpt::maps::CMultiMetricMap::getAs3DObject
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
Definition: CMultiMetricMap.cpp:412
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:135
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:373
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: CParticleFilter.h:223
TPrevious::id
CHMHMapNode::TNodeID id
Definition: CHierarchicalMapMHPartition.cpp:729
mrpt::poses::CPose3DPDFGaussian::mean
CPose3D mean
The mean value.
Definition: CPose3DPDFGaussian.h:79
CRobotPosesGraph.h
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:334
mrpt::hmtslam::CHMHMapArc::Ptr
std::shared_ptr< CHMHMapArc > Ptr
Definition: CHMHMapArc.h:38
mrpt::utils::CMHPropertiesValuesList::const_iterator
std::vector< TPropertyValueIDTriplet >::const_iterator const_iterator
Definition: CMHPropertiesValuesList.h:204
mrpt::system
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:30
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:289
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:60
mrpt::hmtslam
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
Definition: CHierarchicalMapMHPartition.h:30
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: CColouredPointsMap.h:112
iterator
Scalar * iterator
Definition: eigen_plugins.h:26
ops_containers.h
This file implements several operations that operate element-wise on individual or pairs of container...
mrpt::poses::CPose3DPDFSOG
Declares a class that represents a Probability Density function (PDF) of a 3D(6D) pose ...
Definition: CPose3DPDFSOG.h:35
CDisk.h
mrpt::poses::CPose3D::pitch
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:539
mrpt::poses::CPose3D::yaw
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:533
std
STL namespace.
CGridPlaneXY.h
const_iterator
const Scalar * const_iterator
Definition: eigen_plugins.h:27
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
CText.h
s
GLdouble s
Definition: glext.h:3676
obj
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
TDistance::operator=
const TDistance & operator=(const unsigned &D)
Definition: CHierarchicalMapMHPartition.cpp:717
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::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:53
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), computed as a weighted average over all m_particles.
Definition: CPose3DPDFParticles.cpp:75
mrpt::utils::CStringList
A class for storing a list of text lines.
Definition: CStringList.h:32
mrpt::bayes::CParticleFilterData::m_particles
CParticleList m_particles
The array of particles.
Definition: CParticleFilterData.h:208
mrpt::math
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
CSetOfObjects.h
samples
GLsizei samples
Definition: glext.h:8068
MRPT_END
#define MRPT_END
Definition: CColouredPointsMap.h:430
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
Definition: CColouredPointsMap.h:366
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:422
mrpt::utils::CStringList::clear
void clear()
Clear the whole list.
Definition: CStringList.cpp:73
mrpt::opengl::CSetOfObjects::Ptr
std::shared_ptr< CSetOfObjects > Ptr
Definition: CSetOfObjects.h:32
mrpt::maps::CMultiMetricMap::Ptr
std::shared_ptr< CMultiMetricMap > Ptr
Definition: CMultiMetricMap.h:143
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::utils::TNodeID
uint64_t TNodeID
The type for node IDs in graphs of different types.
Definition: types_simple.h:49
mrpt::opengl::COpenGLScene::clear
void clear(bool createMainViewport=true)
Clear the list of objects and viewports in the scene, deleting objects&#39; memory, and leaving just the ...
Definition: COpenGLScene.cpp:62
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, where each row contains a (x,y,phi) datum.
Definition: CPose3DPDFGaussian.cpp:385
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:128
NODE_ANNOTATION_METRIC_MAPS
#define NODE_ANNOTATION_METRIC_MAPS
Definition: HMT_SLAM_common.h:18
string
GLsizei const GLchar ** string
Definition: glext.h:4101
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
mrpt::poses::CPose3D::roll
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:545
CPose3DPDFParticles.h
mrpt::utils::CSerializable::Ptr
std::shared_ptr< CSerializable > Ptr
Definition: CSerializable.h:47
mrpt::poses::CPose3DPDFGaussian::cov
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
Definition: CPose3DPDFGaussian.h:83
CSphere.h
MRPT_START
#define MRPT_START
Definition: CColouredPointsMap.h:426
RAD2DEG
#define RAD2DEG
Definition: CColouredPointsMap.h:115
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:17
CSimpleLine.h
ARC_ANNOTATION_DELTA_SRC_POSEID
#define ARC_ANNOTATION_DELTA_SRC_POSEID
Definition: HMT_SLAM_common.h:27
mrpt::graphslam::optimize_graph_spa_levmarq
void optimize_graph_spa_levmarq(GRAPH_T &graph, TResultInfoSpaLevMarq &out_info, const std::set< mrpt::utils::TNodeID > *in_nodes_to_optimize=nullptr, const mrpt::utils::TParametersDouble &extra_params=mrpt::utils::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:79
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::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
mrpt::poses::CPose3DPDFParticles::resetDeterministic
void resetDeterministic(const CPose3D &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:381
TDistance::TDistance
TDistance(const unsigned &D)
Definition: CHierarchicalMapMHPartition.cpp:716
id
GLuint id
Definition: glext.h:3909
mrpt::graphslam::TResultInfoSpaLevMarq
Output information for mrpt::graphslam::optimize_graph_spa_levmarq()
Definition: graphslam/include/mrpt/graphslam/types.h:68
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::opengl
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
mrpt::opengl::COpenGLScene
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
Definition: COpenGLScene.h:60
source
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
ASSERT_
#define ASSERT_(f)
Definition: CColouredPointsMap.h:310
NODE_ANNOTATION_REF_POSEID
#define NODE_ANNOTATION_REF_POSEID
Definition: HMT_SLAM_common.h:19
mrpt::poses::CPose3DPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
Definition: CPose3DPDFGaussian.h:40
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:244
mrpt::hmtslam::CHMHMapNode::Ptr
std::shared_ptr< CHMHMapNode > Ptr
Definition: CHMHMapNode.h:42
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:225
format
GLenum GLsizei GLenum format
Definition: glext.h:3531
mrpt::graphs::CNetworkOfPoses::root
mrpt::utils::TNodeID root
The ID of the node that is the origin of coordinates, used as reference by all coordinates in nodes...
Definition: CNetworkOfPoses.h:230
size
GLsizeiptr size
Definition: glext.h:3923
mrpt::opengl::CDisk::Ptr
std::shared_ptr< CDisk > Ptr
Definition: CDisk.h:35
mrpt::opengl::CText::Ptr
std::shared_ptr< CText > Ptr
Definition: CText.h:40
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::maps::CMultiMetricMap
This class stores any customizable set of metric maps.
Definition: CMultiMetricMap.h:141
NODE_ANNOTATION_POSES_GRAPH
#define NODE_ANNOTATION_POSES_GRAPH
Definition: HMT_SLAM_common.h:20
Eigen::aligned_allocator
Definition: aligned_containers.h:22
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:25
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:189
mrpt::opengl::CSimpleLine::Ptr
std::shared_ptr< CSimpleLine > Ptr
Definition: CSimpleLine.h:24
ASSERTMSG_
#define ASSERTMSG_(f, __ERROR_MSG)
Definition: CColouredPointsMap.h:307
ARC_ANNOTATION_DELTA_TRG_POSEID
#define ARC_ANNOTATION_DELTA_TRG_POSEID
Definition: HMT_SLAM_common.h:29
mrpt::opengl::CSphere::Ptr
std::shared_ptr< CSphere > Ptr
Definition: CSphere.h:33
a
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
p
GLfloat GLfloat p
Definition: glext.h:6305
mrpt::hmtslam::TArcList
A class for storing a sequence of arcs (a path).
Definition: HMT_SLAM_common.h:106
mrpt::aligned_containers::vector_t
std::vector< TYPE1, Eigen::aligned_allocator< TYPE1 > > vector_t
Definition: aligned_containers.h:33
ARC_ANNOTATION_DELTA
#define ARC_ANNOTATION_DELTA
Definition: HMT_SLAM_common.h:24
mrpt::opengl::CGridPlaneXY::Ptr
std::shared_ptr< CGridPlaneXY > Ptr
Definition: CGridPlaneXY.h:34
mrpt::poses::CPose3DPDFParticles
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
Definition: CPose3DPDFParticles.h:31
mrpt::hmtslam::CHMHMapNode::TNodeID
mrpt::utils::TNodeID TNodeID
The type of the IDs of nodes.
Definition: CHMHMapNode.h:47
mrpt::system::os::_strcmpi
int _strcmpi(const char *str1, const char *str2) noexcept
An OS-independent version of strcmpi.
Definition: os.cpp:319


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019