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(size_t particleIdx) const
508 {
509  if (particleIdx >= m_particles.size())
510  THROW_EXCEPTION("Particle index out of bounds!");
511 
512  auto it = m_particles[particleIdx].d->robotPoses.find(m_currentRobotPose);
513  ASSERT_(it != m_particles[particleIdx].d->robotPoses.end());
514  return &it->second;
515 }
516 
517 /*---------------------------------------------------------------
518  getCurrentPose
519  ---------------------------------------------------------------*/
520 CPose3D* CLocalMetricHypothesis::getCurrentPose(size_t particleIdx)
521 {
522  if (particleIdx >= m_particles.size())
523  THROW_EXCEPTION("Particle index out of bounds!");
524 
525  auto it = m_particles[particleIdx].d->robotPoses.find(m_currentRobotPose);
526  ASSERT_(it != m_particles[particleIdx].d->robotPoses.end());
527  return &it->second;
528 }
529 
530 /*---------------------------------------------------------------
531  getRelativePose
532  ---------------------------------------------------------------*/
533 void CLocalMetricHypothesis::getRelativePose(
534  const TPoseID& reference, const TPoseID& pose,
535  CPose3DPDFParticles& outPDF) const
536 {
537  MRPT_START
538 
539  // Resize output:
540  outPDF.resetDeterministic(TPose3D(0, 0, 0, 0, 0, 0), m_particles.size());
541 
542  CParticleList::const_iterator it;
543  CPose3DPDFParticles::CParticleList::iterator itP;
544  for (it = m_particles.begin(), itP = outPDF.m_particles.begin();
545  it != m_particles.end(); it++, itP++)
546  {
547  itP->log_w = it->log_w;
548 
549  auto srcPose = it->d->robotPoses.find(reference);
550  auto trgPose = it->d->robotPoses.find(pose);
551 
552  ASSERT_(srcPose != it->d->robotPoses.end());
553  ASSERT_(trgPose != it->d->robotPoses.end());
554  itP->d =
555  (CPose3D(trgPose->second) - CPose3D(srcPose->second)).asTPose();
556  }
557 
558  MRPT_END
559 }
560 
561 /*---------------------------------------------------------------
562  changeCoordinateOrigin
563  ---------------------------------------------------------------*/
564 void CLocalMetricHypothesis::changeCoordinateOrigin(const TPoseID& newOrigin)
565 {
566  CPose3DPDFParticles originPDF(m_particles.size());
567 
568  CParticleList::iterator it;
569  CPose3DPDFParticles::CParticleList::iterator itOrgPDF;
570 
571  for (it = m_particles.begin(), itOrgPDF = originPDF.m_particles.begin();
572  it != m_particles.end(); it++, itOrgPDF++)
573  {
574  auto refPoseIt = it->d->robotPoses.find(newOrigin);
575  ASSERT_(refPoseIt != it->d->robotPoses.end());
576  const CPose3D& refPose = refPoseIt->second;
577 
578  // Save in pdf to compute mean:
579  itOrgPDF->d = refPose.asTPose();
580  itOrgPDF->log_w = it->log_w;
581 
582  auto End = it->d->robotPoses.end();
583  // Change all other poses first:
584  for (auto itP = it->d->robotPoses.begin(); itP != End; ++itP)
585  if (itP != refPoseIt) itP->second = itP->second - refPose;
586 
587  // Now set new origin to 0:
588  refPoseIt->second.setFromValues(0, 0, 0);
589  }
590 
591  // Rebuild metric maps for consistency:
592  rebuildMetricMaps();
593 
594  // Change coords in incr. partitioning as well:
595  {
596  std::lock_guard<std::mutex> locker(m_robotPosesGraph.lock);
597 
598  CSimpleMap* SFseq = m_robotPosesGraph.partitioner.getSequenceOfFrames();
599  for (auto& p : m_robotPosesGraph.idx2pose)
600  {
601  CPose3DPDF::Ptr pdf;
603  SFseq->get(p.first, pdf, sf);
604 
605  // Copy from particles:
606  ASSERT_(pdf->GetRuntimeClass() == CLASS_ID(CPose3DPDFParticles));
607  auto pdfParts = std::dynamic_pointer_cast<CPose3DPDFParticles>(pdf);
608  getPoseParticles(p.second, *pdfParts);
609  }
610  }
611 }
612 
613 /*---------------------------------------------------------------
614  rebuildMetricMaps
615  ---------------------------------------------------------------*/
616 void CLocalMetricHypothesis::rebuildMetricMaps()
617 {
618  for (auto& m_particle : m_particles)
619  {
620  m_particle.d->metricMaps.clear();
621 
622  // Follow all robot poses:
623  auto End = m_particle.d->robotPoses.end();
624  for (auto itP = m_particle.d->robotPoses.begin(); itP != End; ++itP)
625  {
626  if (itP->first !=
627  m_currentRobotPose) // Current robot pose has no SF stored.
628  {
629  auto SFit = m_SFs.find(itP->first);
630  ASSERT_(SFit != m_SFs.end());
631  SFit->second.insertObservationsInto(
632  &m_particle.d->metricMaps, &itP->second);
633  }
634  }
635  }
636 }
637 
638 /** Removes a given area from the LMH:
639  * - The corresponding node in the HMT map is updated with the robot poses &
640  *SFs in the LMH.
641  * - Robot poses belonging to that area are removed from:
642  * - the particles.
643  * - the graph partitioner.
644  * - the list of SFs.
645  * - the list m_nodeIDmemberships.
646  * - The weights of all particles are changed to remove the effects of the
647  *removed metric observations.
648  * - After calling this the metric maps should be updated.
649  */
650 void CLocalMetricHypothesis::removeAreaFromLMH(
651  const CHMHMapNode::TNodeID areaID)
652 {
653  MRPT_START
654 
655  // Remove from m_neighbors:
656  // -----------------------------------
657  auto itNeig = m_neighbors.find(areaID);
658  if (itNeig != m_neighbors.end()) m_neighbors.erase(itNeig);
659 
660  // Build the list with the poses in the area to be removed from LMH:
661  // ----------------------------------------------------------------------
662  TNodeIDList lstPoseIDs;
663  for (auto& m_nodeIDmembership : m_nodeIDmemberships)
664  if (m_nodeIDmembership.second == areaID)
665  lstPoseIDs.insert(m_nodeIDmembership.first);
666 
667  ASSERT_(!lstPoseIDs.empty());
668 
669  // ----------------------------------------------------------------------
670  // The corresponding node in the HMT map is updated with the
671  // robot poses & SFs in the LMH.
672  // ----------------------------------------------------------------------
673  updateAreaFromLMH(areaID, true);
674 
675  // - Robot poses belonging to that area are removed from:
676  // - the particles.
677  // ----------------------------------------------------------------------
678  for (auto it = lstPoseIDs.begin(); it != lstPoseIDs.end(); ++it)
679  for (auto& m_particle : m_particles)
680  m_particle.d->robotPoses.erase(m_particle.d->robotPoses.find(*it));
681 
682  // - The weights of all particles are changed to remove the effects of the
683  // removed metric observations.
684  // ----------------------------------------------------------------------
685  {
686  CParticleList::iterator p;
687  vector<map<TPoseID, double>>::iterator ws_it;
688  ASSERT_(m_log_w_metric_history.size() == m_particles.size());
689 
690  for (ws_it = m_log_w_metric_history.begin(), p = m_particles.begin();
691  p != m_particles.end(); ++p, ++ws_it)
692  {
693  for (auto it = lstPoseIDs.begin(); it != lstPoseIDs.end(); ++it)
694  {
695  auto itW = ws_it->find(*it);
696  if (itW != ws_it->end())
697  {
698  MRPT_CHECK_NORMAL_NUMBER(itW->second);
699 
700  p->log_w -= itW->second;
701  // No longer required:
702  ws_it->erase(itW);
703  }
704  }
705  }
706  }
707 
708  // - Robot poses belonging to that area are removed from:
709  // - the graph partitioner.
710  // ----------------------------------------------------------------------
711  {
712  std::lock_guard<std::mutex> locker(m_robotPosesGraph.lock);
713 
714  std::vector<uint32_t> indexesToRemove;
715  indexesToRemove.reserve(lstPoseIDs.size());
716 
717  for (auto it = m_robotPosesGraph.idx2pose.begin();
718  it != m_robotPosesGraph.idx2pose.end();)
719  {
720  if (lstPoseIDs.find(it->second) != lstPoseIDs.end())
721  {
722  indexesToRemove.push_back(it->first);
723 
724  // Remove from the mapping indexes->nodeIDs as well:
725  auto it2 = it;
726  it2++;
727  // it = m_robotPosesGraph.idx2pose.erase( it );
728  m_robotPosesGraph.idx2pose.erase(it);
729  it = it2;
730  }
731  else
732  it++;
733  }
734 
735  m_robotPosesGraph.partitioner.removeSetOfNodes(indexesToRemove);
736 
737  // Renumbering of indexes<->posesIDs to be the same than in
738  // "m_robotPosesGraph.partitioner":
739  unsigned idx = 0;
740  map<uint32_t, TPoseID> newList;
741  for (auto i = m_robotPosesGraph.idx2pose.begin();
742  i != m_robotPosesGraph.idx2pose.end(); ++i, idx++)
743  newList[idx] = i->second;
744  m_robotPosesGraph.idx2pose = newList;
745  }
746 
747  // - Robot poses belonging to that area are removed from:
748  // - the list of SFs.
749  // ----------------------------------------------------------------------
750  // Already done above.
751 
752  // - Robot poses belonging to that area are removed from:
753  // - the list m_nodeIDmemberships.
754  // ----------------------------------------------------------------------
755  for (auto it = lstPoseIDs.begin(); it != lstPoseIDs.end(); ++it)
756  m_nodeIDmemberships.erase(m_nodeIDmemberships.find(*it));
757 
758  double out_max_log_w;
759  normalizeWeights(&out_max_log_w); // Normalize weights:
760 
761  MRPT_END
762 }
763 
764 /*---------------------------------------------------------------
765  TRobotPosesPartitioning::pose2idx
766  ---------------------------------------------------------------*/
767 unsigned int CLocalMetricHypothesis::TRobotPosesPartitioning::pose2idx(
768  const TPoseID& id) const
769 {
770  for (auto it : idx2pose)
771  if (it.second == id) return it.first;
772  THROW_EXCEPTION_FMT("PoseID=%i not found.", static_cast<int>(id));
773 }
774 
775 /*---------------------------------------------------------------
776  updateAreaFromLMH
777 
778 // The corresponding node in the HMT map is updated with the
779 // robot poses & SFs in the LMH.
780  ---------------------------------------------------------------*/
781 void CLocalMetricHypothesis::updateAreaFromLMH(
782  const CHMHMapNode::TNodeID areaID, bool eraseSFsFromLMH)
783 {
784  // Build the list with the poses belonging to that area from LMH:
785  // ----------------------------------------------------------------------
786  TNodeIDList lstPoseIDs;
787  for (auto it = m_nodeIDmemberships.begin(); it != m_nodeIDmemberships.end();
788  ++it)
789  if (it->second == areaID) lstPoseIDs.insert(it->first);
790 
791  ASSERT_(!lstPoseIDs.empty());
792 
793  CHMHMapNode::Ptr node;
794  {
795  std::lock_guard<std::mutex>(m_parent->m_map_cs);
796  node = m_parent->m_map.getNodeByID(areaID);
797  ASSERT_(node);
798  ASSERT_(node->m_hypotheses.has(m_ID));
799  } // end of HMT map cs
800 
801  // The pose to become the origin:
802  TPoseID poseID_origin;
803  node->m_annotations.getElemental(
804  NODE_ANNOTATION_REF_POSEID, poseID_origin, m_ID, true);
805 
806  // 1) The set of robot poses and SFs
807  // In annotation: NODE_ANNOTATION_POSES_GRAPH
808  // ---------------------------------------------------------------------
809  CRobotPosesGraph::Ptr posesGraph;
810  {
811  CSerializable::Ptr annot =
812  node->m_annotations.get(NODE_ANNOTATION_POSES_GRAPH, m_ID);
813  if (!annot)
814  {
815  // Add it now:
816  posesGraph = std::make_shared<CRobotPosesGraph>();
817  node->m_annotations.setMemoryReference(
818  NODE_ANNOTATION_POSES_GRAPH, posesGraph, m_ID);
819  }
820  else
821  {
822  posesGraph = std::dynamic_pointer_cast<CRobotPosesGraph>(annot);
823  posesGraph->clear();
824  }
825  }
826 
827  // For each pose in the area:
828  CPose3DPDFParticles pdfOrigin;
829  bool pdfOrigin_ok = false;
830  for (auto it = lstPoseIDs.begin(); it != lstPoseIDs.end(); ++it)
831  {
832  TPoseInfo& poseInfo = (*posesGraph)[*it];
833  getPoseParticles(*it, poseInfo.pdf); // Save pose particles
834 
835  // Save the PDF of the origin:
836  if (*it == poseID_origin)
837  {
838  pdfOrigin.copyFrom(poseInfo.pdf);
839  pdfOrigin_ok = true;
840  }
841 
842  if (*it != m_currentRobotPose) // The current robot pose has no SF
843  {
844  auto itSF = m_SFs.find(*it);
845  ASSERT_(itSF != m_SFs.end());
846 
847  if (eraseSFsFromLMH)
848  {
849  poseInfo.sf = std::move(itSF->second);
850  m_SFs.erase(itSF);
851  }
852  else
853  {
854  poseInfo.sf = itSF->second; // Copy observations
855  }
856  }
857  }
858 
859  // Readjust to set the origin pose ID:
860  ASSERT_(pdfOrigin_ok);
861  CPose3DPDFParticles pdfOriginInv;
862  pdfOrigin.inverse(pdfOriginInv);
863  for (auto it = posesGraph->begin(); it != posesGraph->end(); ++it)
864  {
865  CPose3DPDFParticles::CParticleList::iterator orgIt, pdfIt;
866  ASSERT_(it->second.pdf.size() == pdfOriginInv.size());
867  for (pdfIt = it->second.pdf.m_particles.begin(),
868  orgIt = pdfOriginInv.m_particles.begin();
869  orgIt != pdfOriginInv.m_particles.end(); orgIt++, pdfIt++)
870  pdfIt->d = (CPose3D(orgIt->d) + CPose3D(pdfIt->d)).asTPose();
871  }
872 
873  // 2) One single metric map built from the most likelily robot poses
874  // In annotation: NODE_ANNOTATION_METRIC_MAPS
875  // ---------------------------------------------------------------------
876  CMultiMetricMap::Ptr metricMap = node->m_annotations.getAs<CMultiMetricMap>(
877  NODE_ANNOTATION_METRIC_MAPS, m_ID, false);
878  metricMap->clear();
879  posesGraph->insertIntoMetricMap(*metricMap);
880 }
881 
882 /*---------------------------------------------------------------
883  dumpAsText
884  ---------------------------------------------------------------*/
885 void CLocalMetricHypothesis::dumpAsText(std::vector<std::string>& st) const
886 {
887  st.clear();
888  st.emplace_back("LIST OF POSES IN LMH");
889  st.emplace_back("====================");
890 
891  string s;
892  s = "Neighbors: ";
893  for (unsigned long m_neighbor : m_neighbors)
894  s += format("%i ", (int)m_neighbor);
895  st.push_back(s);
896 
897  TMapPoseID2Pose3D lst;
898  getMeans(lst);
899 
900  for (auto it = lst.begin(); it != lst.end(); ++it)
901  {
902  auto area = m_nodeIDmemberships.find(it->first);
903  st.emplace_back(mrpt::format(
904  " ID: %i \t AREA: %i \t %.03f,%.03f,%.03fdeg", (int)it->first,
905  (int)area->second, it->second.x(), it->second.y(),
906  RAD2DEG(it->second.yaw())));
907  }
908 }
909 
910 /*---------------------------------------------------------------
911  readFromStream
912  ---------------------------------------------------------------*/
913 void CLocalMetricHypothesis::serializeFrom(
914  mrpt::serialization::CArchive& in, uint8_t version)
915 {
916  switch (version)
917  {
918  case 0:
919  {
920  in >> m_ID >> m_currentRobotPose >> m_neighbors >>
921  m_nodeIDmemberships >> m_SFs >> m_posesPendingAddPartitioner >>
922  m_areasPendingTBI >> m_log_w >> m_log_w_metric_history >>
923  m_robotPosesGraph.partitioner >> m_robotPosesGraph.idx2pose;
924 
925  // particles:
926  readParticlesFromStream(in);
927  }
928  break;
929  default:
931  };
932 }
933 
934 uint8_t CLocalMetricHypothesis::serializeGetVersion() const { return 0; }
935 void CLocalMetricHypothesis::serializeTo(
937 {
938  out << m_ID << m_currentRobotPose << m_neighbors << m_nodeIDmemberships
939  << m_SFs << m_posesPendingAddPartitioner << m_areasPendingTBI << m_log_w
940  << m_log_w_metric_history << m_robotPosesGraph.partitioner
941  << m_robotPosesGraph.idx2pose;
942 
943  // particles:
944  writeParticlesToStream(out);
945 }
946 
947 void CLSLAMParticleData::serializeFrom(
948  mrpt::serialization::CArchive& in, uint8_t version)
949 {
950  switch (version)
951  {
952  case 0:
953  {
954  in >> metricMaps >> robotPoses;
955  }
956  break;
957  default:
959  };
960 }
961 
962 uint8_t CLSLAMParticleData::serializeGetVersion() const { return 0; }
963 void CLSLAMParticleData::serializeTo(mrpt::serialization::CArchive& out) const
964 {
965  out << metricMaps << robotPoses;
966 }
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:775
#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: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.
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 1.9.9 Git: 24b95e159 Thu Jan 23 01:15:46 2020 +0100 at jue ene 23 01:30:10 CET 2020