MRPT  1.9.9
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-2019, 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>
13 #include <mrpt/opengl/CEllipsoid.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.4, 0.4, 0.4);
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  {
127  opengl::CEllipsoid::Ptr ellip = std::make_shared<opengl::CEllipsoid>();
128  // Color depending on being into the current area:
129  if (m_nodeIDmemberships.find(it->first)->second ==
130  m_nodeIDmemberships.find(m_currentRobotPose)->second)
131  ellip->setColor(0, 0, 1);
132  else
133  ellip->setColor(1, 0, 0);
134 
135  const CPose3DPDFParticles* pdfParts = &it->second;
136  CPose3DPDFGaussian pdf;
137  pdf.copyFrom(*pdfParts);
138 
139  // Mean:
140  ellip->setLocation(
141  pdf.mean.x(), pdf.mean.y(),
142  pdf.mean.z() + 0.005); // To be above the ground gridmap...
143 
144  // Cov:
145  CMatrixDouble C = CMatrixDouble(pdf.cov); // 6x6 cov.
146  C.setSize(3, 3);
147 
148  // Is a 2D pose??
149  if (pdf.mean.pitch() == 0 && pdf.mean.roll() == 0 &&
150  pdf.cov(2, 2) < 1e-4f)
151  C.setSize(2, 2);
152 
153  ellip->setCovMatrix(C);
154 
155  ellip->enableDrawSolid3D(false);
156 
157  // Name:
158  ellip->setName(format("P%u", (unsigned int)it->first));
159  ellip->enableShowName();
160 
161  // Add it:
162  objs->insert(ellip);
163 
164  // Add an arrow for the mean direction also:
165  {
167  0, 0, 0, 0.20f, 0, 0, 0.25f, 0.005f, 0.02f);
168  obj->setColor(1, 0, 0);
169  obj->setPose(pdf.mean);
170  objs->insert(obj);
171  }
172 
173  // Is this the current robot pose?
174  if (it->first == m_currentRobotPose)
175  {
176  // Draw robot:
178  robot->setPose(pdf.mean);
179 
180  // Add it:
181  objs->insert(robot);
182  }
183  else // The current robot pose does not count
184  {
185  // Compute the area means:
186  auto itAreaId = m_nodeIDmemberships.find(it->first);
187  ASSERT_(itAreaId != m_nodeIDmemberships.end());
188  CHMHMapNode::TNodeID areaId = itAreaId->second;
189  areas_howmany[areaId]++;
190  areas_mean[areaId].x_incr(pdf.mean.x());
191  areas_mean[areaId].y_incr(pdf.mean.y());
192  areas_mean[areaId].z_incr(pdf.mean.z());
193  }
194  } // end for it
195 
196  // Average area means:
197  map<CHMHMapNode::TNodeID, CPoint3D>::iterator itMeans;
198  map<CHMHMapNode::TNodeID, int>::iterator itHowMany;
199  ASSERT_(areas_howmany.size() == areas_mean.size());
200  for (itMeans = areas_mean.begin(), itHowMany = areas_howmany.begin();
201  itMeans != areas_mean.end(); itMeans++, itHowMany++)
202  {
203  ASSERT_(itHowMany->second > 0);
204 
205  float f = 1.0f / itHowMany->second;
206  itMeans->second *= f;
207  }
208 
209  // -------------------------------------------------------------------
210  // Draw lines between robot poses & their corresponding area sphere
211  // -------------------------------------------------------------------
212  for (it = lstPoses.begin(); it != lstPoses.end(); it++)
213  {
214  if (it->first != m_currentRobotPose)
215  {
216  CPoint3D areaPnt(
217  areas_mean[m_nodeIDmemberships.find(it->first)->second]);
218  areaPnt.z_incr(m_parent->m_options.VIEW3D_AREA_SPHERES_HEIGHT);
219 
220  const CPose3DPDFParticles* pdfParts = &it->second;
221  CPose3DPDFGaussian pdf;
222  pdf.copyFrom(*pdfParts);
223 
225  std::make_shared<opengl::CSimpleLine>();
226  line->setColor(0.8, 0.8, 0.8, 0.3);
227  line->setLineWidth(2);
228 
229  line->setLineCoords(
230  pdf.mean.x(), pdf.mean.y(), pdf.mean.z(), areaPnt.x(),
231  areaPnt.y(), areaPnt.z());
232  objs->insert(line);
233  }
234  } // end for it
235 
236  // -------------------------------------------------------------------
237  // Draw lines for links between robot poses
238  // -------------------------------------------------------------------
239  // for (it=m_robotPoses.begin(); it!=m_robotPoses.end();it++)
240  /* for (it=lstPoses.begin(); it!=lstPoses.end();it++)
241  {
242  const CPose3DPDFParticles *myPdfParts = &it->second;
243  CPose3DPDFGaussian myPdf;
244  myPdf.copyFrom( *myPdfParts );
245 
246  std::map<TPoseID,TInterRobotPosesInfo>::const_iterator itLink;
247  for
248  (itLink=it->second.m_links.begin();itLink!=it->second.m_links.end();itLink++)
249  {
250  if (itLink->second.SSO>0.7)
251  {
252  CRobotPosesAuxiliaryGraph::const_iterator hisIt =
253  m_robotPoses.find( itLink->first );
254  ASSERT_( hisIt !=m_robotPoses.end() );
255 
256  const CPose3DPDFGaussian *hisPdf = & hisIt->second.m_pose;
257 
258  opengl::CSimpleLine::Ptr line =
259  std::make_shared<opengl::CSimpleLine>();
260  line->m_color_R = 0.2f;
261  line->m_color_G = 0.8f;
262  line->m_color_B = 0.2f;
263  line->m_color_A = 0.3f;
264  line->m_lineWidth = 3.0f;
265 
266  line->m_x0 = myPdf->mean.x;
267  line->m_y0 = myPdf->mean.y;
268  line->m_z0 = myPdf->mean.z;
269 
270  line->m_x1 = hisPdf->mean.x;
271  line->m_y1 = hisPdf->mean.y;
272  line->m_z1 = hisPdf->mean.z;
273 
274  objs->insert( line );
275  }
276  }
277  } // end for it
278  */
279 
280  // ---------------------------------------------------------
281  // Draw each of the areas in the neighborhood:
282  // ---------------------------------------------------------
283  {
284  std::lock_guard<std::mutex> lock(
285  m_parent->m_map_cs); // To access nodes' labels.
286 
287  for (itMeans = areas_mean.begin(); itMeans != areas_mean.end();
288  itMeans++)
289  {
290  opengl::CSphere::Ptr sphere = std::make_shared<opengl::CSphere>();
291 
292  if (itMeans->first ==
293  m_nodeIDmemberships.find(m_currentRobotPose)->second)
294  { // Color of current area
295  sphere->setColor(0.1, 0.1, 0.7);
296  }
297  else
298  { // Color of other areas
299  sphere->setColor(0.7, 0, 0);
300  }
301 
302  sphere->setLocation(
303  itMeans->second.x(), itMeans->second.y(),
304  itMeans->second.z() +
305  m_parent->m_options.VIEW3D_AREA_SPHERES_HEIGHT);
306 
307  sphere->setRadius(m_parent->m_options.VIEW3D_AREA_SPHERES_RADIUS);
308 
309  // Add it:
310  objs->insert(sphere);
311 
312  // And text label:
313  opengl::CText::Ptr txt = std::make_shared<opengl::CText>();
314  txt->setColor(1, 1, 1);
315 
316  const CHMHMapNode::Ptr node =
317  m_parent->m_map.getNodeByID(itMeans->first);
318  ASSERT_(node);
319 
320  txt->setLocation(
321  itMeans->second.x(), itMeans->second.y(),
322  itMeans->second.z() +
323  m_parent->m_options.VIEW3D_AREA_SPHERES_HEIGHT);
324 
325  // txt->m_str = node->m_label;
326  txt->setString(format("%li", (long int)node->getID()));
327 
328  objs->insert(txt);
329  }
330  } // end of lock on map_cs
331 
332  // ---------------------------------------------------------
333  // Draw links between areas:
334  // ---------------------------------------------------------
335  {
336  std::lock_guard<std::mutex> lock(m_parent->m_map_cs);
337 
338  for (itMeans = areas_mean.begin(); itMeans != areas_mean.end();
339  itMeans++)
340  {
341  CHMHMapNode::TNodeID srcAreaID = itMeans->first;
342  const CHMHMapNode::Ptr srcArea =
343  m_parent->m_map.getNodeByID(srcAreaID);
344  ASSERT_(srcArea);
345 
346  TArcList lstArcs;
347  srcArea->getArcs(lstArcs);
348  for (auto a = lstArcs.begin(); a != lstArcs.end(); ++a)
349  {
350  // target is in the neighborhood of LMH:
351  if ((*a)->getNodeFrom() == srcAreaID)
352  {
353  auto trgAreaPoseIt = areas_mean.find((*a)->getNodeTo());
354  if (trgAreaPoseIt != areas_mean.end())
355  {
356  // Yes, target node of the arc is in the LMH: Draw it:
358  std::make_shared<opengl::CSimpleLine>();
359  line->setColor(0.8, 0.8, 0);
360  line->setLineWidth(3);
361 
362  line->setLineCoords(
363  areas_mean[srcAreaID].x(),
364  areas_mean[srcAreaID].y(),
365  areas_mean[srcAreaID].z() +
366  m_parent->m_options.VIEW3D_AREA_SPHERES_HEIGHT,
367  trgAreaPoseIt->second.x(),
368  trgAreaPoseIt->second.y(),
369  trgAreaPoseIt->second.z() +
370  m_parent->m_options.VIEW3D_AREA_SPHERES_HEIGHT);
371 
372  objs->insert(line);
373  }
374  }
375  } // end for each arc
376  } // end for each area
377 
378  } // end of lock on map_cs
379 }
380 
381 /** The PF algorithm implementation. */
382 void CLocalMetricHypothesis::prediction_and_update_pfAuxiliaryPFOptimal(
383  const mrpt::obs::CActionCollection* action,
384  const mrpt::obs::CSensoryFrame* observation,
386 {
387  ASSERT_(m_parent.get());
388  ASSERT_(m_parent->m_LSLAM_method);
389  m_parent->m_LSLAM_method->prediction_and_update_pfAuxiliaryPFOptimal(
390  this, action, observation, PF_options);
391 }
392 
393 void CLocalMetricHypothesis::prediction_and_update_pfOptimalProposal(
394  const mrpt::obs::CActionCollection* action,
395  const mrpt::obs::CSensoryFrame* observation,
397 {
398  ASSERT_(m_parent.get());
399  ASSERT_(m_parent->m_LSLAM_method);
400  m_parent->m_LSLAM_method->prediction_and_update_pfOptimalProposal(
401  this, action, observation, PF_options);
402 }
403 
404 /*---------------------------------------------------------------
405  getMeans
406  Returns the mean of each robot pose in this LMH, as
407  computed from the set of particles.
408  ---------------------------------------------------------------*/
409 void CLocalMetricHypothesis::getMeans(TMapPoseID2Pose3D& outList) const
410 {
411  MRPT_START
412 
413  outList.clear();
414 
415  // Build list of particles pdfs:
416  std::map<TPoseID, CPose3DPDFParticles> parts;
417  getPathParticles(parts);
418 
419  std::map<TPoseID, CPose3DPDFParticles>::iterator it;
420 
421  for (it = parts.begin(); it != parts.end(); it++)
422  it->second.getMean(outList[it->first]);
423 
424  MRPT_END
425 }
426 
427 /*---------------------------------------------------------------
428  getPathParticles
429  ---------------------------------------------------------------*/
430 void CLocalMetricHypothesis::getPathParticles(
431  std::map<TPoseID, CPose3DPDFParticles>& outList) const
432 {
433  MRPT_START
434 
435  outList.clear();
436 
437  if (m_particles.empty()) return;
438 
439  // For each poseID:
440  for (auto itPoseID = m_particles.begin()->d->robotPoses.begin();
441  itPoseID != m_particles.begin()->d->robotPoses.end(); ++itPoseID)
442  {
443  CPose3DPDFParticles auxPDF(m_particles.size());
444  CParticleList::const_iterator it;
445  CPose3DPDFParticles::CParticleList::iterator itP;
446  for (it = m_particles.begin(), itP = auxPDF.m_particles.begin();
447  it != m_particles.end(); it++, itP++)
448  {
449  itP->log_w = it->log_w;
450  itP->d = it->d->robotPoses.find(itPoseID->first)->second.asTPose();
451  }
452 
453  // Save PDF:
454  outList[itPoseID->first] = auxPDF;
455  } // end for itPoseID
456 
457  MRPT_END
458 }
459 
460 /*---------------------------------------------------------------
461  getPoseParticles
462  ---------------------------------------------------------------*/
463 void CLocalMetricHypothesis::getPoseParticles(
464  const TPoseID& poseID, CPose3DPDFParticles& outPDF) const
465 {
466  MRPT_START
467 
468  ASSERT_(!m_particles.empty());
469 
470  CParticleList::const_iterator it;
471  outPDF.resetDeterministic(TPose3D(0, 0, 0, 0, 0, 0), m_particles.size());
472  CPose3DPDFParticles::CParticleList::iterator itP;
473  for (it = m_particles.begin(), itP = outPDF.m_particles.begin();
474  it != m_particles.end(); it++, itP++)
475  {
476  itP->log_w = it->log_w;
477  auto itPose = it->d->robotPoses.find(poseID);
478  ASSERT_(itPose != it->d->robotPoses.end());
479  itP->d = itPose->second.asTPose();
480  }
481 
482  MRPT_END
483 }
484 
485 /*---------------------------------------------------------------
486  clearRobotPoses
487  ---------------------------------------------------------------*/
488 void CLocalMetricHypothesis::clearRobotPoses()
489 {
490  clearParticles();
491  m_particles.resize(m_parent->m_options.pf_options.sampleSize);
492  for (auto& m_particle : m_particles)
493  {
494  // Create particle:
495  m_particle.log_w = 0;
496  m_particle.d.reset(new CLSLAMParticleData(
497  &m_parent->m_options.defaultMapsInitializers));
498 
499  // Fill in:
500  m_particle.d->robotPoses.clear();
501  }
502 }
503 
504 /*---------------------------------------------------------------
505  getCurrentPose
506  ---------------------------------------------------------------*/
507 const CPose3D* CLocalMetricHypothesis::getCurrentPose(
508  const 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(const 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>(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(
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(
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:765
#define MRPT_START
Definition: exceptions.h:241
CPose3D mean
The mean value.
GLdouble GLdouble z
Definition: glext.h:3879
double RAD2DEG(const double x)
Radians to degrees.
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
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:548
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:542
STL namespace.
GLdouble s
Definition: glext.h:3682
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
Definition: CHMTSLAM.h:67
Declares a class for storing a collection of robot actions.
unsigned char uint8_t
Definition: rptypes.h:44
#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:89
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:554
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:58
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:53
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:84
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.
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:23
GLuint in
Definition: glext.h:7391
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:3542
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:3542
This class stores any customizable set of metric maps.
#define NODE_ANNOTATION_POSES_GRAPH
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
GLubyte GLubyte GLubyte a
Definition: glext.h:6372
GLfloat GLfloat p
Definition: glext.h:6398
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 1.9.9 Git: ee555d257 Fri Aug 16 10:05:39 2019 +0200 at vie ago 16 10:10:14 CEST 2019