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


Page generated by Doxygen 1.8.14 for MRPT 1.5.6 Git: 4c65e8431 Tue Apr 24 08:18:17 2018 +0200 at lun oct 28 01:35:26 CET 2019