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 
13 #include <mrpt/opengl/CDisk.h>
14 #include <mrpt/opengl/CText.h>
17 #include <mrpt/opengl/CSphere.h>
19 #include <mrpt/system/os.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(
50 {
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 {
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 {
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 {
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 
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  {
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;
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;
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 
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;
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;
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;
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  {
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<
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
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
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  {
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:
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:
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<
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:
1371  TParametersDouble graphslam_params;
1372  graphslam_params["max_iterations"] = numberOfIterations;
1373 
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:
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
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 
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 
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  {
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 }
A namespace of pseudo-random numbers generators of diferent distributions.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
Scalar * iterator
Definition: eigen_plugins.h:26
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:76
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
#define MRPT_START
Definition: exceptions.h:262
CHMHMapNode::TNodeID id
CPose3D mean
The mean value.
void dijkstra_nodes_estimate()
Spanning tree computation of a simple estimation of the global coordinates of each node just from the...
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
mrpt::graphs::TNodeID TNodeID
The type of the IDs of nodes.
Definition: CHMHMapNode.h:45
CParticleList m_particles
The array of particles.
double normalizeWeights(double *out_max_log_w=nullptr) override
Normalize the (logarithmic) weights, such as the maximum weight is zero.
mrpt::graphs::TNodeID root
The ID of the node that is the origin of coordinates, used as reference by all coordinates in nodes...
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
This file implements several operations that operate element-wise on individual or pairs of container...
Declares a class that represents a Probability Density function (PDF) of a 3D(6D) pose ...
Definition: CPose3DPDFSOG.h:32
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:532
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:526
STL namespace.
#define AREAID_INVALID
std::vector< T, mrpt::aligned_allocator_cpp11< T > > aligned_std_vector
GLdouble s
Definition: glext.h:3676
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
const TDistance & operator=(const unsigned &D)
A directed graph of pose constraints, with edges being the relative poses between pairs of nodes iden...
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
void getMean(CPose3D &mean_pose) const override
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF), computed as a weighted average over all m_particles.
This base provides a set of functions for maths stuff.
GLsizei samples
Definition: glext.h:8068
std::vector< TPropertyValueIDTriplet >::const_iterator const_iterator
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
Auxiliary class used in mrpt::slam::CLocalMetricHypothesis for HMT-SLAM; this class stores a set of r...
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
void clear(bool createMainViewport=true)
Clear the list of objects and viewports in the scene, deleting objects&#39; memory, and leaving just the ...
void drawManySamples(size_t N, std::vector< mrpt::math::CVectorDouble > &outSamples) const override
Draws a number of samples from the distribution, and saves as a list of 1x6 vectors, where each row contains a (x,y,phi) datum.
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
#define NODE_ANNOTATION_METRIC_MAPS
GLsizei const GLchar ** string
Definition: glext.h:4101
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:538
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define ARC_ANNOTATION_DELTA_SRC_POSEID
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:38
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
TDistance(const unsigned &D)
GLuint id
Definition: glext.h:3909
#define MRPT_END
Definition: exceptions.h:266
Output information for mrpt::graphslam::optimize_graph_spa_levmarq()
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
Definition: COpenGLScene.h:59
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
typename edges_map_t::const_iterator const_iterator
#define NODE_ANNOTATION_REF_POSEID
uint64_t TNodeID
A generic numeric type for unique IDs of nodes or entities.
Definition: TNodeID.h:16
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
void normalizeAngles()
Rebuild the internal matrix & update the yaw/pitch/roll angles within the ]-PI,PI] range (Must be cal...
Definition: CPose3D.cpp:235
global_poses_t nodes
The nodes (vertices) of the graph, with their estimated "global" (with respect to root) position...
GLenum GLsizei GLenum format
Definition: glext.h:3531
void resetDeterministic(const mrpt::math::TPose3D &location, size_t particlesCount=0)
Reset the PDF to a single point: All m_particles will be set exactly to the supplied pose...
GLsizeiptr size
Definition: glext.h:3923
void insertEdgeAtEnd(TNodeID from_nodeID, TNodeID to_nodeID, const edge_t &edge_value)
Insert an edge (from -> to) with the given edge value (more efficient version to be called if you kno...
This class stores any customizable set of metric maps.
#define NODE_ANNOTATION_POSES_GRAPH
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
void insert(const CRenderizable::Ptr &newObject, const std::string &viewportName=std::string("main"))
Insert a new object into the scene, in the given viewport (by default, into the "main" viewport)...
#define ARC_ANNOTATION_DELTA_TRG_POSEID
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
GLfloat GLfloat p
Definition: glext.h:6305
const Scalar * const_iterator
Definition: eigen_plugins.h:27
A class for storing a sequence of arcs (a path).
#define ARC_ANNOTATION_DELTA
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
int _strcmpi(const char *str1, const char *str2) noexcept
An OS-independent version of strcmpi.
Definition: os.cpp:320



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020