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



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018