MRPT  1.9.9
CLocalMetricHypothesis.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/system/os.h>
16 #include <mrpt/opengl/CEllipsoid.h>
18 #include <mrpt/opengl/CText.h>
19 #include <mrpt/opengl/CSphere.h>
20 #include <mrpt/opengl/CArrow.h>
22 
24 
25 using namespace mrpt;
26 using namespace mrpt::slam;
27 using namespace mrpt::hmtslam;
28 using namespace mrpt::opengl;
29 using namespace mrpt::obs;
30 using namespace mrpt::maps;
31 using namespace mrpt::poses;
32 using namespace mrpt::math;
33 using namespace std;
34 
37 
38 /*---------------------------------------------------------------
39  Normal constructor
40  ---------------------------------------------------------------*/
42  : m_parent(parent), m_log_w_metric_history()
43 // m_log_w_topol_history()
44 {
45  m_log_w = 0;
46  m_accumRobotMovementIsValid = false;
47 }
48 
49 /*---------------------------------------------------------------
50  Destructor
51  ---------------------------------------------------------------*/
52 CLocalMetricHypothesis::~CLocalMetricHypothesis() { clearParticles(); }
53 /*---------------------------------------------------------------
54  getAs3DScene
55 
56  Returns a 3D representation of the current robot pose, all
57  the poses in the auxiliary graph, and each of the areas
58  they belong to.
59  ---------------------------------------------------------------*/
60 void CLocalMetricHypothesis::getAs3DScene(
61  opengl::CSetOfObjects::Ptr& objs) const
62 {
63  objs->clear();
64 
65  // -------------------------------------------
66  // Draw a grid on the ground:
67  // -------------------------------------------
68  {
70  mrpt::make_aligned_shared<opengl::CGridPlaneXY>(
71  -100, 100, -100, 100, 0, 5);
72  obj->setColor(0.4, 0.4, 0.4);
73 
74  objs->insert(obj); // it will free the memory
75  }
76 
77  // ---------------------------------------------------------
78  // Draw each of the robot poses as 2D/3D ellipsoids
79  // For the current pose, draw the robot itself as well.
80  // ---------------------------------------------------------
81  std::map<TPoseID, CPose3DPDFParticles> lstPoses;
83  getPathParticles(lstPoses);
84 
85  // -----------------------------------------------
86  // Draw a 3D coordinates corner for each cluster
87  // -----------------------------------------------
88  {
89  std::lock_guard<std::mutex> lock(m_parent->m_map_cs);
90 
91  for (TNodeIDSet::const_iterator n = m_neighbors.begin();
92  n != m_neighbors.end(); ++n)
93  {
94  const CHMHMapNode::Ptr node = m_parent->m_map.getNodeByID(*n);
95  ASSERT_(node);
96  TPoseID poseID_origin;
97  CPose3D originPose;
98  if (node->m_annotations.getElemental(
99  NODE_ANNOTATION_REF_POSEID, poseID_origin, m_ID))
100  {
101  lstPoses[poseID_origin].getMean(originPose);
102 
104  corner->setPose(originPose);
105  objs->insert(corner);
106  }
107  }
108  } // end of lock on map_cs
109 
110  // Add a camera following the robot:
111  // -----------------------------------------
112  const CPose3D meanCurPose = lstPoses[m_currentRobotPose].getMeanVal();
113  {
114  opengl::CCamera::Ptr cam = mrpt::make_aligned_shared<opengl::CCamera>();
115  cam->setZoomDistance(85);
116  cam->setAzimuthDegrees(45 + RAD2DEG(meanCurPose.yaw()));
117  cam->setElevationDegrees(45);
118  cam->setPointingAt(meanCurPose);
119  objs->insert(cam);
120  }
121 
122  map<CHMHMapNode::TNodeID, CPoint3D>
123  areas_mean; // Store the mean pose of each area
124  map<CHMHMapNode::TNodeID, int> areas_howmany;
125 
126  for (it = lstPoses.begin(); it != lstPoses.end(); it++)
127  {
129  mrpt::make_aligned_shared<opengl::CEllipsoid>();
130  // Color depending on being into the current area:
131  if (m_nodeIDmemberships.find(it->first)->second ==
132  m_nodeIDmemberships.find(m_currentRobotPose)->second)
133  ellip->setColor(0, 0, 1);
134  else
135  ellip->setColor(1, 0, 0);
136 
137  const CPose3DPDFParticles* pdfParts = &it->second;
138  CPose3DPDFGaussian pdf;
139  pdf.copyFrom(*pdfParts);
140 
141  // Mean:
142  ellip->setLocation(
143  pdf.mean.x(), pdf.mean.y(),
144  pdf.mean.z() + 0.005); // To be above the ground gridmap...
145 
146  // Cov:
147  CMatrixDouble C = CMatrixDouble(pdf.cov); // 6x6 cov.
148  C.setSize(3, 3);
149 
150  // Is a 2D pose??
151  if (pdf.mean.pitch() == 0 && pdf.mean.roll() == 0 &&
152  pdf.cov(2, 2) < 1e-4f)
153  C.setSize(2, 2);
154 
155  ellip->setCovMatrix(C);
156 
157  ellip->enableDrawSolid3D(false);
158 
159  // Name:
160  ellip->setName(format("P%u", (unsigned int)it->first));
161  ellip->enableShowName();
162 
163  // Add it:
164  objs->insert(ellip);
165 
166  // Add an arrow for the mean direction also:
167  {
169  0, 0, 0, 0.20f, 0, 0, 0.25f, 0.005f, 0.02f);
170  obj->setColor(1, 0, 0);
171  obj->setPose(pdf.mean);
172  objs->insert(obj);
173  }
174 
175  // Is this the current robot pose?
176  if (it->first == m_currentRobotPose)
177  {
178  // Draw robot:
180  robot->setPose(pdf.mean);
181 
182  // Add it:
183  objs->insert(robot);
184  }
185  else // The current robot pose does not count
186  {
187  // Compute the area means:
189  m_nodeIDmemberships.find(it->first);
190  ASSERT_(itAreaId != m_nodeIDmemberships.end());
191  CHMHMapNode::TNodeID areaId = itAreaId->second;
192  areas_howmany[areaId]++;
193  areas_mean[areaId].x_incr(pdf.mean.x());
194  areas_mean[areaId].y_incr(pdf.mean.y());
195  areas_mean[areaId].z_incr(pdf.mean.z());
196  }
197  } // end for it
198 
199  // Average area means:
202  ASSERT_(areas_howmany.size() == areas_mean.size());
203  for (itMeans = areas_mean.begin(), itHowMany = areas_howmany.begin();
204  itMeans != areas_mean.end(); itMeans++, itHowMany++)
205  {
206  ASSERT_(itHowMany->second > 0);
207 
208  float f = 1.0f / itHowMany->second;
209  itMeans->second *= f;
210  }
211 
212  // -------------------------------------------------------------------
213  // Draw lines between robot poses & their corresponding area sphere
214  // -------------------------------------------------------------------
215  for (it = lstPoses.begin(); it != lstPoses.end(); it++)
216  {
217  if (it->first != m_currentRobotPose)
218  {
219  CPoint3D areaPnt(
220  areas_mean[m_nodeIDmemberships.find(it->first)->second]);
221  areaPnt.z_incr(m_parent->m_options.VIEW3D_AREA_SPHERES_HEIGHT);
222 
223  const CPose3DPDFParticles* pdfParts = &it->second;
224  CPose3DPDFGaussian pdf;
225  pdf.copyFrom(*pdfParts);
226 
228  mrpt::make_aligned_shared<opengl::CSimpleLine>();
229  line->setColor(0.8, 0.8, 0.8, 0.3);
230  line->setLineWidth(2);
231 
232  line->setLineCoords(
233  pdf.mean.x(), pdf.mean.y(), pdf.mean.z(), areaPnt.x(),
234  areaPnt.y(), areaPnt.z());
235  objs->insert(line);
236  }
237  } // end for it
238 
239  // -------------------------------------------------------------------
240  // Draw lines for links between robot poses
241  // -------------------------------------------------------------------
242  // for (it=m_robotPoses.begin(); it!=m_robotPoses.end();it++)
243  /* for (it=lstPoses.begin(); it!=lstPoses.end();it++)
244  {
245  const CPose3DPDFParticles *myPdfParts = &it->second;
246  CPose3DPDFGaussian myPdf;
247  myPdf.copyFrom( *myPdfParts );
248 
249  std::map<TPoseID,TInterRobotPosesInfo>::const_iterator itLink;
250  for
251  (itLink=it->second.m_links.begin();itLink!=it->second.m_links.end();itLink++)
252  {
253  if (itLink->second.SSO>0.7)
254  {
255  CRobotPosesAuxiliaryGraph::const_iterator hisIt =
256  m_robotPoses.find( itLink->first );
257  ASSERT_( hisIt !=m_robotPoses.end() );
258 
259  const CPose3DPDFGaussian *hisPdf = & hisIt->second.m_pose;
260 
261  opengl::CSimpleLine::Ptr line =
262  mrpt::make_aligned_shared<opengl::CSimpleLine>();
263  line->m_color_R = 0.2f;
264  line->m_color_G = 0.8f;
265  line->m_color_B = 0.2f;
266  line->m_color_A = 0.3f;
267  line->m_lineWidth = 3.0f;
268 
269  line->m_x0 = myPdf->mean.x;
270  line->m_y0 = myPdf->mean.y;
271  line->m_z0 = myPdf->mean.z;
272 
273  line->m_x1 = hisPdf->mean.x;
274  line->m_y1 = hisPdf->mean.y;
275  line->m_z1 = hisPdf->mean.z;
276 
277  objs->insert( line );
278  }
279  }
280  } // end for it
281  */
282 
283  // ---------------------------------------------------------
284  // Draw each of the areas in the neighborhood:
285  // ---------------------------------------------------------
286  {
287  std::lock_guard<std::mutex> lock(
288  m_parent->m_map_cs); // To access nodes' labels.
289 
290  for (itMeans = areas_mean.begin(); itMeans != areas_mean.end();
291  itMeans++)
292  {
293  opengl::CSphere::Ptr sphere =
294  mrpt::make_aligned_shared<opengl::CSphere>();
295 
296  if (itMeans->first ==
297  m_nodeIDmemberships.find(m_currentRobotPose)->second)
298  { // Color of current area
299  sphere->setColor(0.1, 0.1, 0.7);
300  }
301  else
302  { // Color of other areas
303  sphere->setColor(0.7, 0, 0);
304  }
305 
306  sphere->setLocation(
307  itMeans->second.x(), itMeans->second.y(),
308  itMeans->second.z() +
309  m_parent->m_options.VIEW3D_AREA_SPHERES_HEIGHT);
310 
311  sphere->setRadius(m_parent->m_options.VIEW3D_AREA_SPHERES_RADIUS);
312 
313  // Add it:
314  objs->insert(sphere);
315 
316  // And text label:
317  opengl::CText::Ptr txt = mrpt::make_aligned_shared<opengl::CText>();
318  txt->setColor(1, 1, 1);
319 
320  const CHMHMapNode::Ptr node =
321  m_parent->m_map.getNodeByID(itMeans->first);
322  ASSERT_(node);
323 
324  txt->setLocation(
325  itMeans->second.x(), itMeans->second.y(),
326  itMeans->second.z() +
327  m_parent->m_options.VIEW3D_AREA_SPHERES_HEIGHT);
328 
329  // txt->m_str = node->m_label;
330  txt->setString(format("%li", (long int)node->getID()));
331 
332  objs->insert(txt);
333  }
334  } // end of lock on map_cs
335 
336  // ---------------------------------------------------------
337  // Draw links between areas:
338  // ---------------------------------------------------------
339  {
340  std::lock_guard<std::mutex> lock(m_parent->m_map_cs);
341 
342  for (itMeans = areas_mean.begin(); itMeans != areas_mean.end();
343  itMeans++)
344  {
345  CHMHMapNode::TNodeID srcAreaID = itMeans->first;
346  const CHMHMapNode::Ptr srcArea =
347  m_parent->m_map.getNodeByID(srcAreaID);
348  ASSERT_(srcArea);
349 
350  TArcList lstArcs;
351  srcArea->getArcs(lstArcs);
352  for (TArcList::const_iterator a = lstArcs.begin();
353  a != lstArcs.end(); ++a)
354  {
355  // target is in the neighborhood of LMH:
356  if ((*a)->getNodeFrom() == srcAreaID)
357  {
359  trgAreaPoseIt = areas_mean.find((*a)->getNodeTo());
360  if (trgAreaPoseIt != areas_mean.end())
361  {
362  // Yes, target node of the arc is in the LMH: Draw it:
364  mrpt::make_aligned_shared<opengl::CSimpleLine>();
365  line->setColor(0.8, 0.8, 0);
366  line->setLineWidth(3);
367 
368  line->setLineCoords(
369  areas_mean[srcAreaID].x(),
370  areas_mean[srcAreaID].y(),
371  areas_mean[srcAreaID].z() +
372  m_parent->m_options.VIEW3D_AREA_SPHERES_HEIGHT,
373  trgAreaPoseIt->second.x(),
374  trgAreaPoseIt->second.y(),
375  trgAreaPoseIt->second.z() +
376  m_parent->m_options.VIEW3D_AREA_SPHERES_HEIGHT);
377 
378  objs->insert(line);
379  }
380  }
381  } // end for each arc
382  } // end for each area
383 
384  } // end of lock on map_cs
385 }
386 
387 /** The PF algorithm implementation. */
388 void CLocalMetricHypothesis::prediction_and_update_pfAuxiliaryPFOptimal(
389  const mrpt::obs::CActionCollection* action,
390  const mrpt::obs::CSensoryFrame* observation,
392 {
393  ASSERT_(m_parent.get());
394  ASSERT_(m_parent->m_LSLAM_method);
395  m_parent->m_LSLAM_method->prediction_and_update_pfAuxiliaryPFOptimal(
396  this, action, observation, PF_options);
397 }
398 
399 void CLocalMetricHypothesis::prediction_and_update_pfOptimalProposal(
400  const mrpt::obs::CActionCollection* action,
401  const mrpt::obs::CSensoryFrame* observation,
403 {
404  ASSERT_(m_parent.get());
405  ASSERT_(m_parent->m_LSLAM_method);
406  m_parent->m_LSLAM_method->prediction_and_update_pfOptimalProposal(
407  this, action, observation, PF_options);
408 }
409 
410 /*---------------------------------------------------------------
411  getMeans
412  Returns the mean of each robot pose in this LMH, as
413  computed from the set of particles.
414  ---------------------------------------------------------------*/
415 void CLocalMetricHypothesis::getMeans(TMapPoseID2Pose3D& outList) const
416 {
417  MRPT_START
418 
419  outList.clear();
420 
421  // Build list of particles pdfs:
422  std::map<TPoseID, CPose3DPDFParticles> parts;
423  getPathParticles(parts);
424 
426 
427  for (it = parts.begin(); it != parts.end(); it++)
428  it->second.getMean(outList[it->first]);
429 
430  MRPT_END
431 }
432 
433 /*---------------------------------------------------------------
434  getPathParticles
435  ---------------------------------------------------------------*/
436 void CLocalMetricHypothesis::getPathParticles(
437  std::map<TPoseID, CPose3DPDFParticles>& outList) const
438 {
439  MRPT_START
440 
441  outList.clear();
442 
443  if (m_particles.empty()) return;
444 
445  // For each poseID:
446  for (TMapPoseID2Pose3D::const_iterator itPoseID =
447  m_particles.begin()->d->robotPoses.begin();
448  itPoseID != m_particles.begin()->d->robotPoses.end(); ++itPoseID)
449  {
450  CPose3DPDFParticles auxPDF(m_particles.size());
453  for (it = m_particles.begin(), itP = auxPDF.m_particles.begin();
454  it != m_particles.end(); it++, itP++)
455  {
456  itP->log_w = it->log_w;
457  itP->d = it->d->robotPoses.find(itPoseID->first)->second.asTPose();
458  }
459 
460  // Save PDF:
461  outList[itPoseID->first] = auxPDF;
462  } // end for itPoseID
463 
464  MRPT_END
465 }
466 
467 /*---------------------------------------------------------------
468  getPoseParticles
469  ---------------------------------------------------------------*/
470 void CLocalMetricHypothesis::getPoseParticles(
471  const TPoseID& poseID, CPose3DPDFParticles& outPDF) const
472 {
473  MRPT_START
474 
475  ASSERT_(!m_particles.empty());
476 
478  outPDF.resetDeterministic(TPose3D(0, 0, 0, 0, 0, 0), m_particles.size());
480  for (it = m_particles.begin(), itP = outPDF.m_particles.begin();
481  it != m_particles.end(); it++, itP++)
482  {
483  itP->log_w = it->log_w;
485  it->d->robotPoses.find(poseID);
486  ASSERT_(itPose != it->d->robotPoses.end());
487  itP->d = itPose->second.asTPose();
488  }
489 
490  MRPT_END
491 }
492 
493 /*---------------------------------------------------------------
494  clearRobotPoses
495  ---------------------------------------------------------------*/
496 void CLocalMetricHypothesis::clearRobotPoses()
497 {
498  clearParticles();
499  m_particles.resize(m_parent->m_options.pf_options.sampleSize);
500  for (CParticleList::iterator it = m_particles.begin();
501  it != m_particles.end(); ++it)
502  {
503  // Create particle:
504  it->log_w = 0;
505  it->d.reset(new CLSLAMParticleData(
506  &m_parent->m_options.defaultMapsInitializers));
507 
508  // Fill in:
509  it->d->robotPoses.clear();
510  }
511 }
512 
513 /*---------------------------------------------------------------
514  getCurrentPose
515  ---------------------------------------------------------------*/
516 const CPose3D* CLocalMetricHypothesis::getCurrentPose(
517  const size_t& particleIdx) const
518 {
519  if (particleIdx >= m_particles.size())
520  THROW_EXCEPTION("Particle index out of bounds!");
521 
523  m_particles[particleIdx].d->robotPoses.find(m_currentRobotPose);
524  ASSERT_(it != m_particles[particleIdx].d->robotPoses.end());
525  return &it->second;
526 }
527 
528 /*---------------------------------------------------------------
529  getCurrentPose
530  ---------------------------------------------------------------*/
531 CPose3D* CLocalMetricHypothesis::getCurrentPose(const size_t& particleIdx)
532 {
533  if (particleIdx >= m_particles.size())
534  THROW_EXCEPTION("Particle index out of bounds!");
535 
537  m_particles[particleIdx].d->robotPoses.find(m_currentRobotPose);
538  ASSERT_(it != m_particles[particleIdx].d->robotPoses.end());
539  return &it->second;
540 }
541 
542 /*---------------------------------------------------------------
543  getRelativePose
544  ---------------------------------------------------------------*/
545 void CLocalMetricHypothesis::getRelativePose(
546  const TPoseID& reference, const TPoseID& pose,
547  CPose3DPDFParticles& outPDF) const
548 {
549  MRPT_START
550 
551  // Resize output:
552  outPDF.resetDeterministic(TPose3D(0, 0, 0, 0, 0, 0), m_particles.size());
553 
556  for (it = m_particles.begin(), itP = outPDF.m_particles.begin();
557  it != m_particles.end(); it++, itP++)
558  {
559  itP->log_w = it->log_w;
560 
561  auto srcPose = it->d->robotPoses.find(reference);
562  auto trgPose = it->d->robotPoses.find(pose);
563 
564  ASSERT_(srcPose != it->d->robotPoses.end());
565  ASSERT_(trgPose != it->d->robotPoses.end());
566  itP->d =
567  (CPose3D(trgPose->second) - CPose3D(srcPose->second)).asTPose();
568  }
569 
570  MRPT_END
571 }
572 
573 /*---------------------------------------------------------------
574  changeCoordinateOrigin
575  ---------------------------------------------------------------*/
576 void CLocalMetricHypothesis::changeCoordinateOrigin(const TPoseID& newOrigin)
577 {
578  CPose3DPDFParticles originPDF(m_particles.size());
579 
582 
583  for (it = m_particles.begin(), itOrgPDF = originPDF.m_particles.begin();
584  it != m_particles.end(); it++, itOrgPDF++)
585  {
586  auto refPoseIt = it->d->robotPoses.find(newOrigin);
587  ASSERT_(refPoseIt != it->d->robotPoses.end());
588  const CPose3D& refPose = refPoseIt->second;
589 
590  // Save in pdf to compute mean:
591  itOrgPDF->d = refPose.asTPose();
592  itOrgPDF->log_w = it->log_w;
593 
594  TMapPoseID2Pose3D::iterator End = it->d->robotPoses.end();
595  // Change all other poses first:
596  for (TMapPoseID2Pose3D::iterator itP = it->d->robotPoses.begin();
597  itP != End; ++itP)
598  if (itP != refPoseIt) itP->second = itP->second - refPose;
599 
600  // Now set new origin to 0:
601  refPoseIt->second.setFromValues(0, 0, 0);
602  }
603 
604  // Rebuild metric maps for consistency:
605  rebuildMetricMaps();
606 
607  // Change coords in incr. partitioning as well:
608  {
609  std::lock_guard<std::mutex> locker(m_robotPosesGraph.lock);
610 
611  CSimpleMap* SFseq = m_robotPosesGraph.partitioner.getSequenceOfFrames();
613  m_robotPosesGraph.idx2pose.begin();
614  it != m_robotPosesGraph.idx2pose.end(); ++it)
615  {
616  CPose3DPDF::Ptr pdf;
618  SFseq->get(it->first, pdf, sf);
619 
620  // Copy from particles:
621  ASSERT_(pdf->GetRuntimeClass() == CLASS_ID(CPose3DPDFParticles));
622  CPose3DPDFParticles::Ptr pdfParts =
623  std::dynamic_pointer_cast<CPose3DPDFParticles>(pdf);
624  getPoseParticles(it->second, *pdfParts);
625  }
626  }
627 }
628 
629 /*---------------------------------------------------------------
630  rebuildMetricMaps
631  ---------------------------------------------------------------*/
632 void CLocalMetricHypothesis::rebuildMetricMaps()
633 {
634  for (CParticleList::iterator it = m_particles.begin();
635  it != m_particles.end(); ++it)
636  {
637  it->d->metricMaps.clear();
638 
639  // Follow all robot poses:
640  TMapPoseID2Pose3D::iterator End = it->d->robotPoses.end();
641  for (TMapPoseID2Pose3D::iterator itP = it->d->robotPoses.begin();
642  itP != End; ++itP)
643  {
644  if (itP->first !=
645  m_currentRobotPose) // Current robot pose has no SF stored.
646  {
648  m_SFs.find(itP->first);
649  ASSERT_(SFit != m_SFs.end());
650  SFit->second.insertObservationsInto(
651  &it->d->metricMaps, &itP->second);
652  }
653  }
654  }
655 }
656 
657 /** Removes a given area from the LMH:
658  * - The corresponding node in the HMT map is updated with the robot poses &
659  *SFs in the LMH.
660  * - Robot poses belonging to that area are removed from:
661  * - the particles.
662  * - the graph partitioner.
663  * - the list of SFs.
664  * - the list m_nodeIDmemberships.
665  * - The weights of all particles are changed to remove the effects of the
666  *removed metric observations.
667  * - After calling this the metric maps should be updated.
668  */
669 void CLocalMetricHypothesis::removeAreaFromLMH(
670  const CHMHMapNode::TNodeID areaID)
671 {
672  MRPT_START
673 
674  // Remove from m_neighbors:
675  // -----------------------------------
676  TNodeIDSet::iterator itNeig = m_neighbors.find(areaID);
677  if (itNeig != m_neighbors.end()) m_neighbors.erase(itNeig);
678 
679  // Build the list with the poses in the area to be removed from LMH:
680  // ----------------------------------------------------------------------
681  TNodeIDList lstPoseIDs;
683  m_nodeIDmemberships.begin();
684  it != m_nodeIDmemberships.end(); ++it)
685  if (it->second == areaID) lstPoseIDs.insert(it->first);
686 
687  ASSERT_(!lstPoseIDs.empty());
688 
689  // ----------------------------------------------------------------------
690  // The corresponding node in the HMT map is updated with the
691  // robot poses & SFs in the LMH.
692  // ----------------------------------------------------------------------
693  updateAreaFromLMH(areaID, true);
694 
695  // - Robot poses belonging to that area are removed from:
696  // - the particles.
697  // ----------------------------------------------------------------------
698  for (TNodeIDList::const_iterator it = lstPoseIDs.begin();
699  it != lstPoseIDs.end(); ++it)
700  for (CParticleList::iterator p = m_particles.begin();
701  p != m_particles.end(); ++p)
702  p->d->robotPoses.erase(p->d->robotPoses.find(*it));
703 
704  // - The weights of all particles are changed to remove the effects of the
705  // removed metric observations.
706  // ----------------------------------------------------------------------
707  {
709  vector<map<TPoseID, double>>::iterator ws_it;
710  ASSERT_(m_log_w_metric_history.size() == m_particles.size());
711 
712  for (ws_it = m_log_w_metric_history.begin(), p = m_particles.begin();
713  p != m_particles.end(); ++p, ++ws_it)
714  {
715  for (TNodeIDList::const_iterator it = lstPoseIDs.begin();
716  it != lstPoseIDs.end(); ++it)
717  {
718  map<TPoseID, double>::iterator itW = ws_it->find(*it);
719  if (itW != ws_it->end())
720  {
721  MRPT_CHECK_NORMAL_NUMBER(itW->second);
722 
723  p->log_w -= itW->second;
724  // No longer required:
725  ws_it->erase(itW);
726  }
727  }
728  }
729  }
730 
731  // - Robot poses belonging to that area are removed from:
732  // - the graph partitioner.
733  // ----------------------------------------------------------------------
734  {
735  std::lock_guard<std::mutex> locker(m_robotPosesGraph.lock);
736 
737  std::vector<uint32_t> indexesToRemove;
738  indexesToRemove.reserve(lstPoseIDs.size());
739 
741  m_robotPosesGraph.idx2pose.begin();
742  it != m_robotPosesGraph.idx2pose.end();)
743  {
744  if (lstPoseIDs.find(it->second) != lstPoseIDs.end())
745  {
746  indexesToRemove.push_back(it->first);
747 
748  // Remove from the mapping indexes->nodeIDs as well:
750  it2++;
751  // it = m_robotPosesGraph.idx2pose.erase( it );
752  m_robotPosesGraph.idx2pose.erase(it);
753  it = it2;
754  }
755  else
756  it++;
757  }
758 
759  m_robotPosesGraph.partitioner.removeSetOfNodes(indexesToRemove);
760 
761  // Renumbering of indexes<->posesIDs to be the same than in
762  // "m_robotPosesGraph.partitioner":
763  unsigned idx = 0;
764  map<uint32_t, TPoseID> newList;
766  m_robotPosesGraph.idx2pose.begin();
767  i != m_robotPosesGraph.idx2pose.end(); ++i, idx++)
768  newList[idx] = i->second;
769  m_robotPosesGraph.idx2pose = newList;
770  }
771 
772  // - Robot poses belonging to that area are removed from:
773  // - the list of SFs.
774  // ----------------------------------------------------------------------
775  // Already done above.
776 
777  // - Robot poses belonging to that area are removed from:
778  // - the list m_nodeIDmemberships.
779  // ----------------------------------------------------------------------
780  for (TNodeIDList::const_iterator it = lstPoseIDs.begin();
781  it != lstPoseIDs.end(); ++it)
782  m_nodeIDmemberships.erase(m_nodeIDmemberships.find(*it));
783 
784  double out_max_log_w;
785  normalizeWeights(&out_max_log_w); // Normalize weights:
786 
787  MRPT_END
788 }
789 
790 /*---------------------------------------------------------------
791  TRobotPosesPartitioning::pose2idx
792  ---------------------------------------------------------------*/
793 unsigned int CLocalMetricHypothesis::TRobotPosesPartitioning::pose2idx(
794  const TPoseID& id) const
795 {
796  for (std::map<uint32_t, TPoseID>::const_iterator it = idx2pose.begin();
797  it != idx2pose.end(); ++it)
798  if (it->second == id) return it->first;
799  THROW_EXCEPTION_FMT("PoseID=%i not found.", static_cast<int>(id));
800 }
801 
802 /*---------------------------------------------------------------
803  updateAreaFromLMH
804 
805 // The corresponding node in the HMT map is updated with the
806 // robot poses & SFs in the LMH.
807  ---------------------------------------------------------------*/
808 void CLocalMetricHypothesis::updateAreaFromLMH(
809  const CHMHMapNode::TNodeID areaID, bool eraseSFsFromLMH)
810 {
811  // Build the list with the poses belonging to that area from LMH:
812  // ----------------------------------------------------------------------
813  TNodeIDList lstPoseIDs;
815  m_nodeIDmemberships.begin();
816  it != m_nodeIDmemberships.end(); ++it)
817  if (it->second == areaID) lstPoseIDs.insert(it->first);
818 
819  ASSERT_(!lstPoseIDs.empty());
820 
821  CHMHMapNode::Ptr node;
822  {
823  std::lock_guard<std::mutex>(m_parent->m_map_cs);
824  node = m_parent->m_map.getNodeByID(areaID);
825  ASSERT_(node);
826  ASSERT_(node->m_hypotheses.has(m_ID));
827  } // end of HMT map cs
828 
829  // The pose to become the origin:
830  TPoseID poseID_origin;
831  node->m_annotations.getElemental(
832  NODE_ANNOTATION_REF_POSEID, poseID_origin, m_ID, true);
833 
834  // 1) The set of robot poses and SFs
835  // In annotation: NODE_ANNOTATION_POSES_GRAPH
836  // ---------------------------------------------------------------------
837  CRobotPosesGraph::Ptr posesGraph;
838  {
839  CSerializable::Ptr annot =
840  node->m_annotations.get(NODE_ANNOTATION_POSES_GRAPH, m_ID);
841  if (!annot)
842  {
843  // Add it now:
844  posesGraph = mrpt::make_aligned_shared<CRobotPosesGraph>();
845  node->m_annotations.setMemoryReference(
846  NODE_ANNOTATION_POSES_GRAPH, posesGraph, m_ID);
847  }
848  else
849  {
850  posesGraph = std::dynamic_pointer_cast<CRobotPosesGraph>(annot);
851  posesGraph->clear();
852  }
853  }
854 
855  // For each pose in the area:
856  CPose3DPDFParticles pdfOrigin;
857  bool pdfOrigin_ok = false;
858  for (TNodeIDList::const_iterator it = lstPoseIDs.begin();
859  it != lstPoseIDs.end(); ++it)
860  {
861  TPoseInfo& poseInfo = (*posesGraph)[*it];
862  getPoseParticles(*it, poseInfo.pdf); // Save pose particles
863 
864  // Save the PDF of the origin:
865  if (*it == poseID_origin)
866  {
867  pdfOrigin.copyFrom(poseInfo.pdf);
868  pdfOrigin_ok = true;
869  }
870 
871  if (*it != m_currentRobotPose) // The current robot pose has no SF
872  {
873  std::map<TPoseID, CSensoryFrame>::iterator itSF = m_SFs.find(*it);
874  ASSERT_(itSF != m_SFs.end());
875 
876  if (eraseSFsFromLMH)
877  {
878  poseInfo.sf.moveFrom(itSF->second); // This leaves m_SFs[*it]
879  // without observations,
880  // but it is being erased
881  // just now:
882  m_SFs.erase(itSF);
883  }
884  else
885  {
886  poseInfo.sf = itSF->second; // Copy observations
887  }
888  }
889  }
890 
891  // Readjust to set the origin pose ID:
892  ASSERT_(pdfOrigin_ok);
893  CPose3DPDFParticles pdfOriginInv;
894  pdfOrigin.inverse(pdfOriginInv);
895  for (CRobotPosesGraph::iterator it = posesGraph->begin();
896  it != posesGraph->end(); ++it)
897  {
899  ASSERT_(it->second.pdf.size() == pdfOriginInv.size());
900  for (pdfIt = it->second.pdf.m_particles.begin(),
901  orgIt = pdfOriginInv.m_particles.begin();
902  orgIt != pdfOriginInv.m_particles.end(); orgIt++, pdfIt++)
903  pdfIt->d = (CPose3D(orgIt->d) + CPose3D(pdfIt->d)).asTPose();
904  }
905 
906  // 2) One single metric map built from the most likelily robot poses
907  // In annotation: NODE_ANNOTATION_METRIC_MAPS
908  // ---------------------------------------------------------------------
909  CMultiMetricMap::Ptr metricMap = node->m_annotations.getAs<CMultiMetricMap>(
910  NODE_ANNOTATION_METRIC_MAPS, m_ID, false);
911  metricMap->clear();
912  posesGraph->insertIntoMetricMap(*metricMap);
913 }
914 
915 /*---------------------------------------------------------------
916  dumpAsText
917  ---------------------------------------------------------------*/
918 void CLocalMetricHypothesis::dumpAsText(std::vector<std::string>& st) const
919 {
920  st.clear();
921  st.push_back("LIST OF POSES IN LMH");
922  st.push_back("====================");
923 
924  string s;
925  s = "Neighbors: ";
926  for (TNodeIDSet::const_iterator it = m_neighbors.begin();
927  it != m_neighbors.end(); ++it)
928  s += format("%i ", (int)*it);
929  st.push_back(s);
930 
931  TMapPoseID2Pose3D lst;
932  getMeans(lst);
933 
934  for (TMapPoseID2Pose3D::const_iterator it = lst.begin(); it != lst.end();
935  ++it)
936  {
938  m_nodeIDmemberships.find(it->first);
939 
940  string s = format(
941  " ID: %i \t AREA: %i \t %.03f,%.03f,%.03fdeg", (int)it->first,
942  (int)area->second, it->second.x(), it->second.y(),
943  RAD2DEG(it->second.yaw()));
944  st.push_back(s);
945  }
946 }
947 
948 /*---------------------------------------------------------------
949  readFromStream
950  ---------------------------------------------------------------*/
951 void CLocalMetricHypothesis::serializeFrom(
953 {
954  switch (version)
955  {
956  case 0:
957  {
958  in >> m_ID >> m_currentRobotPose >> m_neighbors >>
959  m_nodeIDmemberships >> m_SFs >> m_posesPendingAddPartitioner >>
960  m_areasPendingTBI >> m_log_w >> m_log_w_metric_history >>
961  m_robotPosesGraph.partitioner >> m_robotPosesGraph.idx2pose;
962 
963  // particles:
964  readParticlesFromStream(in);
965  }
966  break;
967  default:
969  };
970 }
971 
972 uint8_t CLocalMetricHypothesis::serializeGetVersion() const { return 0; }
973 void CLocalMetricHypothesis::serializeTo(
975 {
976  out << m_ID << m_currentRobotPose << m_neighbors << m_nodeIDmemberships
977  << m_SFs << m_posesPendingAddPartitioner << m_areasPendingTBI << m_log_w
978  << m_log_w_metric_history << m_robotPosesGraph.partitioner
979  << m_robotPosesGraph.idx2pose;
980 
981  // particles:
982  writeParticlesToStream(out);
983 }
984 
985 void CLSLAMParticleData::serializeFrom(
987 {
988  switch (version)
989  {
990  case 0:
991  {
992  in >> metricMaps >> robotPoses;
993  }
994  break;
995  default:
997  };
998 }
999 
1000 uint8_t CLSLAMParticleData::serializeGetVersion() const { return 0; }
1001 void CLSLAMParticleData::serializeTo(mrpt::serialization::CArchive& out) const
1002 {
1003  out << metricMaps << robotPoses;
1004 }
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:1043
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
Scalar * iterator
Definition: eigen_plugins.h:26
#define MRPT_START
Definition: exceptions.h:262
CPose3D mean
The mean value.
GLdouble GLdouble z
Definition: glext.h:3872
double RAD2DEG(const double x)
Radians to degrees.
std::list< T >::iterator find(const T &i)
#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
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:33
CParticleList m_particles
The array of particles.
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
void moveFrom(CSensoryFrame &sf)
Copies all the observation from another object, then erase them from the origin object (this method i...
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
GLenum GLsizei n
Definition: glext.h:5074
mrpt::poses::CPose3DPDFParticles pdf
The robot pose PDF.
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.
GLdouble s
Definition: glext.h:3676
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
Definition: CHMTSLAM.h:68
Declares a class for storing a collection of robot actions.
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
This base provides a set of functions for maths stuff.
mrpt::aligned_std_map< TPoseID, mrpt::poses::CPose3D > TMapPoseID2Pose3D
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:84
static Ptr Create(Args &&... args)
Definition: CArrow.h:31
#define MRPT_CHECK_NORMAL_NUMBER(v)
Throws an exception if the number is NaN, IND, or +/-INF, or return the same number otherwise...
Definition: exceptions.h:118
CMatrixTemplateNumeric< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
This namespace contains representation of robot actions and observations.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:52
size_t size() const
Get the m_particles count (equivalent to "particlesCount")
Auxiliary class used in mrpt::slam::CLocalMetricHypothesis for HMT-SLAM; this class stores a set of r...
void get(size_t index, mrpt::poses::CPose3DPDF::Ptr &out_posePDF, mrpt::obs::CSensoryFrame::Ptr &out_SF) const
Access to the i&#39;th pair, first one is index &#39;0&#39;.
Definition: CSimpleMap.cpp:56
This class is used in HMT-SLAM to represent each of the Local Metric Hypotheses (LMHs).
#define NODE_ANNOTATION_METRIC_MAPS
A class used to store a 3D point.
Definition: CPoint3D.h:31
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
CSetOfObjects::Ptr RobotPioneer()
Returns a representation of a Pioneer II mobile base.
Auxiliary class used in mrpt::slam::CLocalMetricHypothesis for HMT-SLAM; this class keeps the data re...
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between m_particles and gaussian representations...
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:52
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
CSetOfObjects::Ptr CornerXYZ(float scale=1.0)
Returns three arrows representing a X,Y,Z 3D corner.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
The configuration of a particle filter.
#define MRPT_END
Definition: exceptions.h:266
mrpt::obs::CSensoryFrame sf
The observations.
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
GLuint in
Definition: glext.h:7274
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
#define NODE_ANNOTATION_REF_POSEID
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
Information kept for each robot pose used in CRobotPosesGraph.
GLenum GLint GLint y
Definition: glext.h:3538
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...
void inverse(CPose3DPDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
GLenum GLint x
Definition: glext.h:3538
This class stores any customizable set of metric maps.
#define NODE_ANNOTATION_POSES_GRAPH
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:43
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).
Declares a class that represents a Probability Density function (PDF) of a 3D pose.



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