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



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020