Main MRPT website > C++ reference for MRPT 1.5.6
CMultiMetricMapPDF.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 #include "slam-precomp.h" // Precompiled headers
11 
12 
13 #include <mrpt/random.h>
14 #include <mrpt/utils/CTicTac.h>
15 #include <mrpt/utils/CFileStream.h>
16 #include <mrpt/system/os.h>
17 
27 
29 
30 using namespace mrpt;
31 using namespace mrpt::math;
32 using namespace mrpt::slam;
33 using namespace mrpt::obs;
34 using namespace mrpt::maps;
35 using namespace mrpt::poses;
36 using namespace mrpt::random;
37 using namespace mrpt::utils;
38 using namespace mrpt::system;
39 using namespace std;
40 
43 
44 /*---------------------------------------------------------------
45  Constructor
46  ---------------------------------------------------------------*/
48  const bayes::CParticleFilter::TParticleFilterOptions &opts,
49  const mrpt::maps::TSetOfMetricMapInitializers *mapsInitializers,
50  const TPredictionParams *predictionOptions) :
51  averageMap( mapsInitializers ),
52  averageMapIsUpdated(false),
53  SFs(),
54  SF2robotPath(),
55  options(),
56  newInfoIndex(0)
57 {
58  m_particles.resize( opts.sampleSize );
59  for (CParticleList::iterator it=m_particles.begin();it!=m_particles.end();++it)
60  {
61  it->log_w = 0;
62  it->d.reset(new CRBPFParticleData(mapsInitializers));
63  }
64 
65  // Initialize:
66  const CPose3D nullPose(0,0,0);
67  clear(nullPose);
68 
69  // If provided, copy the whole set of params now:
70  if (predictionOptions!=NULL)
71  options = *predictionOptions;
72 }
73 
74 /*---------------------------------------------------------------
75  clear
76  ---------------------------------------------------------------*/
77 void CMultiMetricMapPDF::clear( const CPose2D &initialPose )
78 {
79  CPose3D p(initialPose);
80  clear(p);
81 }
82 
83 /*---------------------------------------------------------------
84  clear
85  ---------------------------------------------------------------*/
86 void CMultiMetricMapPDF::clear( const CPose3D &initialPose )
87 {
88  const size_t M = m_particles.size();
89  for (size_t i=0;i<M;i++)
90  {
91  m_particles[i].log_w = 0;
92 
93  m_particles[i].d->mapTillNow.clear();
94 
95  m_particles[i].d->robotPath.resize(1);
96  m_particles[i].d->robotPath[0]=initialPose;
97  }
98 
99  SFs.clear();
100  SF2robotPath.clear();
101 
102  averageMapIsUpdated = false;
103 }
104 
106 {
107  const size_t nParts = m_particles.size(), nOldKeyframes = prevMap.size();
108  if(nOldKeyframes == 0)
109  {
110  //prevMap is empty, so reset the map
111  clear(currentPose);
112  return;
113  }
114  for (size_t idxPart = 0; idxPart<nParts; idxPart++)
115  {
116  auto &p = m_particles[idxPart];
117  p.log_w = 0;
118 
119  p.d->mapTillNow.clear();
120 
121  p.d->robotPath.resize(nOldKeyframes);
122  for (size_t i = 0; i < nOldKeyframes; i++)
123  {
124  CPose3DPDFPtr keyframe_pose;
125  CSensoryFramePtr sfkeyframe_sf;
126  prevMap.get(i, keyframe_pose, sfkeyframe_sf);
127 
128  // as pose, use: if the PDF is also a PF with the same number of samples, use those particles;
129  // otherwise, simply use the mean for all particles as an approximation (with loss of uncertainty).
130  mrpt::poses::CPose3D kf_pose;
131  bool kf_pose_set = false;
132  if (IS_CLASS(keyframe_pose, CPose3DPDFParticles))
133  {
134  const auto pdf_parts = dynamic_cast<const CPose3DPDFParticles*>(keyframe_pose.pointer());
135  ASSERT_(pdf_parts);
136  if (pdf_parts->particlesCount() == nParts)
137  {
138  kf_pose = *pdf_parts->m_particles[idxPart].d;
139  kf_pose_set = true;
140  }
141  }
142  if (!kf_pose_set)
143  {
144  kf_pose = keyframe_pose->getMeanVal();
145  }
146  p.d->robotPath[i] = kf_pose;
147  for (const auto &obs : *sfkeyframe_sf)
148  {
149  p.d->mapTillNow.insertObservation(&(*obs), &kf_pose);
150  }
151  }
152  }
153 
154  SFs = prevMap; // copy
155  SF2robotPath.clear();
156  SF2robotPath.reserve(nOldKeyframes);
157  for (size_t i = 0; i < nOldKeyframes; i++)
158  SF2robotPath.push_back(i);
159 
160  averageMapIsUpdated = false;
161 }
162 
163 
164 /*---------------------------------------------------------------
165  getEstimatedPosePDF
166  Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF), computed
167  as a weighted average over all m_particles.
168  ---------------------------------------------------------------*/
169 void CMultiMetricMapPDF::getEstimatedPosePDF( CPose3DPDFParticles &out_estimation ) const
170 {
171  ASSERT_(m_particles[0].d->robotPath.size()>0);
172  getEstimatedPosePDFAtTime( m_particles[0].d->robotPath.size()-1, out_estimation );
173 }
174 
175 /*---------------------------------------------------------------
176  getEstimatedPosePDFAtTime
177  ---------------------------------------------------------------*/
178 void CMultiMetricMapPDF::getEstimatedPosePDFAtTime(
179  size_t timeStep,
180  CPose3DPDFParticles &out_estimation ) const
181 {
182  //CPose3D p;
183  size_t i,n = m_particles.size();
184 
185  // Delete current content of "out_estimation":
186  out_estimation.clearParticles();
187 
188  // Create new m_particles:
189  out_estimation.m_particles.resize(n);
190  for (i=0;i<n;i++)
191  {
192  out_estimation.m_particles[i].d.reset(new CPose3D(m_particles[i].d->robotPath[timeStep]));
193  out_estimation.m_particles[i].log_w = m_particles[i].log_w;
194  }
195 
196 }
197 
198 
199 /*---------------------------------------------------------------
200  writeToStream
201  ---------------------------------------------------------------*/
202 void CRBPFParticleData::writeToStream(mrpt::utils::CStream &out,int *version) const
203 {
204  MRPT_UNUSED_PARAM(out); MRPT_UNUSED_PARAM(version);
205  THROW_EXCEPTION("Shouldn't arrive here")
206 }
207 
208 /*---------------------------------------------------------------
209  readFromStream
210  ---------------------------------------------------------------*/
211 void CRBPFParticleData::readFromStream(mrpt::utils::CStream &in, int version)
212 {
213  MRPT_UNUSED_PARAM(in); MRPT_UNUSED_PARAM(version);
214  THROW_EXCEPTION("Shouldn't arrive here")
215 }
216 
217 /*---------------------------------------------------------------
218  writeToStream
219  ---------------------------------------------------------------*/
220 void CMultiMetricMapPDF::writeToStream(mrpt::utils::CStream &out,int *version) const
221 {
222  if (version)
223  *version = 0;
224  else
225  {
226  uint32_t i,n,j,m;
227 
228  // The data
229  n = static_cast<uint32_t>(m_particles.size());
230  out << n;
231  for (i=0;i<n;i++)
232  {
233  out << m_particles[i].log_w << m_particles[i].d->mapTillNow;
234  m = static_cast<uint32_t>(m_particles[i].d->robotPath.size());
235  out << m;
236  for (j=0;j<m;j++)
237  out << m_particles[i].d->robotPath[j];
238  }
239  out << SFs << SF2robotPath;
240  }
241 }
242 
243 /*---------------------------------------------------------------
244  readFromStream
245  ---------------------------------------------------------------*/
246 void CMultiMetricMapPDF::readFromStream(mrpt::utils::CStream &in, int version)
247 {
248  switch(version)
249  {
250  case 0:
251  {
252  uint32_t i,n,j,m;
253 
254  // Delete current contents:
255  // --------------------------
256  clearParticles();
257  SFs.clear();
258  SF2robotPath.clear();
259 
260  averageMapIsUpdated = false;
261 
262  // Load the new data:
263  // --------------------
264  in >> n;
265 
266  m_particles.resize(n);
267  for (i=0;i<n;i++)
268  {
269  m_particles[i].d.reset(new CRBPFParticleData());
270 
271  // Load
272  in >> m_particles[i].log_w >> m_particles[i].d->mapTillNow;
273 
274  in >> m;
275  m_particles[i].d->robotPath.resize(m);
276  for (j=0;j<m;j++)
277  in >> m_particles[i].d->robotPath[j];
278  }
279 
280  in >> SFs >> SF2robotPath;
281 
282  } break;
283  default:
285 
286  };
287 }
288 
289 /*---------------------------------------------------------------
290  getLastPose
291  ---------------------------------------------------------------*/
292 TPose3D CMultiMetricMapPDF::getLastPose(const size_t i, bool &is_valid_pose) const
293 {
294  if (i>=m_particles.size()) THROW_EXCEPTION("Particle index out of bounds!");
295 
296  if (m_particles[i].d->robotPath.empty())
297  {
298  is_valid_pose = false;
299  return TPose3D(0,0,0,0,0,0);
300  }
301  else
302  {
303  return *m_particles[i].d->robotPath.rbegin();
304  }
305 }
306 
307 const CMultiMetricMap * CMultiMetricMapPDF::getAveragedMetricMapEstimation( )
308 {
309  rebuildAverageMap();
310  return &averageMap;
311 }
312 
313 
314 /*---------------------------------------------------------------
315  getWeightedAveragedMap
316  ---------------------------------------------------------------*/
317 void CMultiMetricMapPDF::rebuildAverageMap()
318 {
319  float min_x = 1e6, max_x=-1e6, min_y = 1e6, max_y = -1e6;
321 
322  if (averageMapIsUpdated)
323  return;
324 
325  // ---------------------------------------------------------
326  // GRID
327  // ---------------------------------------------------------
328  for (part=m_particles.begin();part!=m_particles.end();++part)
329  {
330  ASSERT_( part->d->mapTillNow.m_gridMaps.size()>0 );
331 
332  min_x = min( min_x, part->d->mapTillNow.m_gridMaps[0]->getXMin() );
333  max_x = max( max_x, part->d->mapTillNow.m_gridMaps[0]->getXMax() );
334  min_y = min( min_y, part->d->mapTillNow.m_gridMaps[0]->getYMin() );
335  max_y = max( max_y, part->d->mapTillNow.m_gridMaps[0]->getYMax() );
336  }
337 
338  // Asure all maps have the same dimensions:
339  for (part=m_particles.begin();part!=m_particles.end();++part)
340  part->d->mapTillNow.m_gridMaps[0]->resizeGrid(min_x,max_x,min_y,max_y,0.5f,false);
341 
342  for (part=m_particles.begin();part!=m_particles.end();++part)
343  {
344  min_x = min( min_x, part->d->mapTillNow.m_gridMaps[0]->getXMin() );
345  max_x = max( max_x, part->d->mapTillNow.m_gridMaps[0]->getXMax() );
346  min_y = min( min_y, part->d->mapTillNow.m_gridMaps[0]->getYMin() );
347  max_y = max( max_y, part->d->mapTillNow.m_gridMaps[0]->getYMax() );
348  }
349 
350  // Prepare target map:
351  ASSERT_( averageMap.m_gridMaps.size()>0 );
352  averageMap.m_gridMaps[0]->setSize(
353  min_x,
354  max_x,
355  min_y,
356  max_y,
357  m_particles[0].d->mapTillNow.m_gridMaps[0]->getResolution(),
358  0);
359 
360  // Compute the sum of weights:
361  double sumLinearWeights = 0;
362  for (part=m_particles.begin();part!=m_particles.end();++part)
363  sumLinearWeights += exp( part->log_w );
364 
365  // CHECK:
366  for (part=m_particles.begin();part!=m_particles.end();++part)
367  {
368  ASSERT_(part->d->mapTillNow.m_gridMaps[0]->getSizeX() == averageMap.m_gridMaps[0]->getSizeX());
369  ASSERT_(part->d->mapTillNow.m_gridMaps[0]->getSizeY() == averageMap.m_gridMaps[0]->getSizeY());
370  }
371 
372  {
373  // ******************************************************
374  // Implementation WITHOUT the SSE Instructions Set
375  // ******************************************************
376  MRPT_START
377 
378  // Reserve a float grid-map, add weight all maps
379  // -------------------------------------------------------------------------------------------
380  std::vector<float> floatMap;
381  floatMap.resize(averageMap.m_gridMaps[0]->map.size(),0);
382 
383  // For each particle in the RBPF:
384  double sumW = 0;
385  for (part=m_particles.begin();part!=m_particles.end();++part)
386  sumW+=exp(part->log_w);
387 
388  if (sumW==0) sumW=1;
389 
390  for (part=m_particles.begin();part!=m_particles.end();++part)
391  {
392  // Variables:
394  std::vector<COccupancyGridMap2D::cellType>::iterator firstSrcCell = part->d->mapTillNow.m_gridMaps[0]->map.begin();
395  std::vector<COccupancyGridMap2D::cellType>::iterator lastSrcCell = part->d->mapTillNow.m_gridMaps[0]->map.end();
397 
398  // The weight of particle:
399  float w = exp(part->log_w) / sumW;
400 
401  ASSERT_( part->d->mapTillNow.m_gridMaps[0]->map.size() == floatMap.size() );
402 
403  // For each cell in individual maps:
404  for (srcCell = firstSrcCell, destCell = floatMap.begin();srcCell!=lastSrcCell;srcCell++,destCell++)
405  (*destCell) += w * (*srcCell);
406 
407  }
408 
409  // Copy to fixed point map:
411  std::vector<COccupancyGridMap2D::cellType>::iterator destCell = averageMap.m_gridMaps[0]->map.begin();
412 
413  ASSERT_( averageMap.m_gridMaps[0]->map.size() == floatMap.size() );
414 
415  for (srcCell=floatMap.begin();srcCell!=floatMap.end();srcCell++,destCell++)
416  *destCell = static_cast<COccupancyGridMap2D::cellType>( *srcCell );
417 
418  MRPT_END
419  } // End of SSE not supported
420 
421  // Don't calculate again until really necesary.
422  averageMapIsUpdated = true;
423 }
424 
425 /*---------------------------------------------------------------
426  insertObservation
427  ---------------------------------------------------------------*/
428 bool CMultiMetricMapPDF::insertObservation(CSensoryFrame &sf)
429 {
430  const size_t M = particlesCount();
431 
432  // Insert into SFs:
433  CPose3DPDFParticlesPtr posePDF = CPose3DPDFParticles::Create();
434  getEstimatedPosePDF(*posePDF);
435 
436  // Insert it into the SFs and the SF2robotPath list:
437  const uint32_t new_sf_id = SFs.size();
438  SFs.insert(
439  posePDF,
440  CSensoryFramePtr( new CSensoryFrame(sf) ) );
441  SF2robotPath.resize(new_sf_id+1);
442  SF2robotPath[new_sf_id] = m_particles[0].d->robotPath.size() - 1;
443 
444  bool anymap = false;
445  for (size_t i=0;i<M;i++)
446  {
447  bool pose_is_valid;
448  const CPose3D robotPose = CPose3D(getLastPose(i, pose_is_valid));
449  //ASSERT_(pose_is_valid); // if not, use the default (0,0,0)
450  const bool map_modified = sf.insertObservationsInto( &m_particles[i].d->mapTillNow, &robotPose );
451  anymap = anymap || map_modified;
452  }
453 
454  averageMapIsUpdated = false;
455  return anymap;
456 }
457 
458 /*---------------------------------------------------------------
459  getPath
460  ---------------------------------------------------------------*/
461 void CMultiMetricMapPDF::getPath(size_t i, std::deque<math::TPose3D> &out_path) const
462 {
463  if (i>=m_particles.size())
464  THROW_EXCEPTION("Index out of bounds");
465  out_path = m_particles[i].d->robotPath;
466 }
467 
468 /*---------------------------------------------------------------
469  getCurrentEntropyOfPaths
470  ---------------------------------------------------------------*/
471 double CMultiMetricMapPDF::getCurrentEntropyOfPaths()
472 {
473  size_t i;
474  size_t N=m_particles[0].d->robotPath.size(); // The poses count along the paths
475 
476 
477  // Compute paths entropy:
478  // ---------------------------
479  double H_paths = 0;
480 
481  if (N)
482  {
483  // For each pose along the path:
484  for (i=0;i<N;i++)
485  {
486  // Get pose est. as m_particles:
487  CPose3DPDFParticles posePDFParts;
488  getEstimatedPosePDFAtTime(i,posePDFParts);
489 
490  // Approximate to gaussian and compute entropy of covariance:
491  H_paths+= posePDFParts.getCovarianceEntropy();
492  }
493  H_paths /= N;
494  }
495  return H_paths;
496 }
497 
498 /*---------------------------------------------------------------
499  getCurrentJointEntropy
500  ---------------------------------------------------------------*/
501 double CMultiMetricMapPDF::getCurrentJointEntropy()
502 {
503  double H_joint,H_paths,H_maps;
504  size_t i,M = m_particles.size();
506 
507  // Entropy of the paths:
508  H_paths = getCurrentEntropyOfPaths();
509 
510 
511  float min_x = 1e6, max_x=-1e6, min_y = 1e6, max_y = -1e6;
513 
514  // ---------------------------------------------------------
515  // ASSURE ALL THE GRIDS ARE THE SAME SIZE!
516  // ---------------------------------------------------------
517  for (part=m_particles.begin();part!=m_particles.end();++part)
518  {
519  ASSERT_( part->d->mapTillNow.m_gridMaps.size()>0 );
520 
521  min_x = min( min_x, part->d->mapTillNow.m_gridMaps[0]->getXMin() );
522  max_x = max( max_x, part->d->mapTillNow.m_gridMaps[0]->getXMax() );
523  min_y = min( min_y, part->d->mapTillNow.m_gridMaps[0]->getYMin() );
524  max_y = max( max_y, part->d->mapTillNow.m_gridMaps[0]->getYMax() );
525  }
526 
527  // Asure all maps have the same dimensions:
528  for (part=m_particles.begin();part!=m_particles.end();++part)
529  part->d->mapTillNow.m_gridMaps[0]->resizeGrid(min_x,max_x,min_y,max_y,0.5f,false);
530 
531 
532  // Sum of linear weights:
533  double sumLinearWeights = 0;
534  for (i=0;i<M;i++)
535  sumLinearWeights += exp(m_particles[i].log_w);
536 
537  // Compute weighted maps entropy:
538  // --------------------------------
539  H_maps = 0;
540  for (i=0;i<M;i++)
541  {
542  ASSERT_( m_particles[i].d->mapTillNow.m_gridMaps.size()>0 );
543 
544  m_particles[i].d->mapTillNow.m_gridMaps[0]->computeEntropy( entropy );
545  H_maps += exp(m_particles[i].log_w) * entropy.H / sumLinearWeights;
546  }
547 
548  printf("H_paths=%e\n",H_paths);
549  printf("H_maps=%e\n",H_maps);
550 
551  H_joint = H_paths + H_maps;
552  return H_joint;
553 }
554 
555 const CMultiMetricMap * CMultiMetricMapPDF::getCurrentMostLikelyMetricMap() const
556 {
557  size_t i,max_i=0, n = m_particles.size();
558  double max_w = m_particles[0].log_w;
559 
560  for (i=0;i<n;i++)
561  {
562  if ( m_particles[i].log_w > max_w )
563  {
564  max_w = m_particles[i].log_w;
565  max_i = i;
566  }
567  }
568 
569  // Return its map:
570  return &m_particles[max_i].d->mapTillNow;
571 }
572 
573 /*---------------------------------------------------------------
574  updateSensoryFrameSequence
575  ---------------------------------------------------------------*/
576 void CMultiMetricMapPDF::updateSensoryFrameSequence()
577 {
578  MRPT_START
579  CPose3DPDFParticles posePartsPDF;
580  CPose3DPDFPtr previousPosePDF;
581  CSensoryFramePtr dummy;
582 
583  for (size_t i=0;i<SFs.size();i++)
584  {
585  // Get last estimation:
586  SFs.get(i,previousPosePDF,dummy);
587 
588  // Compute the new one:
589  getEstimatedPosePDFAtTime(SF2robotPath[i], posePartsPDF);
590 
591  // Copy into SFs:
592  previousPosePDF->copyFrom( posePartsPDF );
593  }
594 
595  MRPT_END
596 }
597 
598 /*---------------------------------------------------------------
599  saveCurrentPathEstimationToTextFile
600  ---------------------------------------------------------------*/
601 void CMultiMetricMapPDF::saveCurrentPathEstimationToTextFile( const std::string &fil )
602 {
603  FILE *f=os::fopen( fil.c_str(), "wt");
604  if (!f) return;
605 
606  for (CParticleList::iterator it=m_particles.begin();it!=m_particles.end();++it)
607  {
608  for (size_t i=0;i<it->d->robotPath.size();i++)
609  {
610  const mrpt::math::TPose3D &p = it->d->robotPath[i];
611 
612  os::fprintf(f,"%.04f %.04f %.04f %.04f %.04f %.04f ",
613  p.x,p.y,p.z,
614  p.yaw, p.pitch, p.roll );
615  }
616  os::fprintf(f," %e\n", it->log_w );
617  }
618 
619  os::fclose(f);
620 }
621 
622 /*---------------------------------------------------------------
623  TPredictionParams
624  ---------------------------------------------------------------*/
626  pfOptimalProposal_mapSelection(0),
627  ICPGlobalAlign_MinQuality(0.70f),
628  update_gridMapLikelihoodOptions(),
629  KLD_params(),
630  icp_params()
631 {
632 }
633 
634 /*---------------------------------------------------------------
635  dumpToTextStream
636  ---------------------------------------------------------------*/
638 {
639  out.printf("\n----------- [CMultiMetricMapPDF::TPredictionParams] ------------ \n\n");
640 
641  out.printf("pfOptimalProposal_mapSelection = %i\n", pfOptimalProposal_mapSelection );
642  out.printf("ICPGlobalAlign_MinQuality = %f\n", ICPGlobalAlign_MinQuality );
643 
646  out.printf("\n");
647 }
648 
649 /*---------------------------------------------------------------
650  loadFromConfigFile
651  ---------------------------------------------------------------*/
653  const mrpt::utils::CConfigFileBase &iniFile,
654  const std::string &section)
655 {
656  pfOptimalProposal_mapSelection = iniFile.read_int(section,"pfOptimalProposal_mapSelection",pfOptimalProposal_mapSelection, true);
657 
658  MRPT_LOAD_CONFIG_VAR( ICPGlobalAlign_MinQuality, float, iniFile,section );
659 
660  KLD_params.loadFromConfigFile(iniFile, section);
661  icp_params.loadFromConfigFile(iniFile, section);
662 }
bool insertObservationsInto(mrpt::maps::CMetricMap *theMap, const mrpt::poses::CPose3D *robotPose=NULL) const
Insert all the observations in this SF into a metric map or any kind (see mrpt::maps::CMetricMap).
FILE BASE_IMPEXP * fopen(const char *fileName, const char *mode) MRPT_NO_THROWS
An OS-independent version of fopen.
Definition: os.cpp:255
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
Definition: CICP.cpp:178
#define min(a, b)
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
Definition: CICP.cpp:133
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 dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
Definition: TKLDParams.cpp:37
int BASE_IMPEXP void BASE_IMPEXP fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:272
mrpt::slam::CICP::TConfigParams icp_params
ICP parameters, used only when "PF_algorithm=2" in the particle filter.
double roll
Roll coordinate (rotation angle over X coordinate).
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
float ICPGlobalAlign_MinQuality
[prediction stage][pf optimal proposal only] If useICPGlobalAlign_withGrid=true, this is the minimum ...
#define THROW_EXCEPTION(msg)
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
Scalar * iterator
Definition: eigen_plugins.h:23
GLubyte GLubyte GLubyte GLubyte w
Definition: glew.h:1797
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
int BASE_IMPEXP fprintf(FILE *fil, const char *format,...) MRPT_NO_THROWS MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:412
double yaw
Yaw coordinate (rotation angle over Z axis).
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:113
GLuint in
Definition: glew.h:7146
This class allows loading and storing values and vectors of different types from a configuration text...
size_t size() const
Returns the count of pairs (pose,sensory data)
Definition: CSimpleMap.cpp:67
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
CParticleList m_particles
The array of particles.
double H
The target variable for absolute entropy, computed as: H(map)=Sumx,y{ -p(x,y)*ln(p(x,y)) -(1-p(x,y))*ln(1-p(x,y)) }
GLsizei n
Definition: glew.h:5051
#define MRPT_END
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
double getCovarianceEntropy() const
Compute the entropy of the estimated covariance matrix.
int version
Definition: mrpt_jpeglib.h:898
GLfloat GLfloat p
Definition: glew.h:10113
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
Auxiliary class used in mrpt::maps::CMultiMetricMapPDF.
double pitch
Pitch coordinate (rotation angle over Y axis).
#define MRPT_START
#define MRPT_LOAD_CONFIG_VAR(variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
GLsizei const GLcharARB ** string
Definition: glew.h:3293
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
static CPose3DPDFParticlesPtr Create()
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::utils::CSerializable) is of t...
Definition: CObject.h:103
The struct for passing extra simulation parameters to the prediction/update stage when running a part...
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
#define ASSERT_(f)
const GLdouble * m
Definition: glew.h:5094
void clearParticles()
Free the memory of all the particles and reset the array "m_particles" to length zero.
int pfOptimalProposal_mapSelection
[pf optimal proposal only] Only for PF algorithm=2 (Exact "pfOptimalProposal") Select the map on whic...
This class stores any customizable set of metric maps.
unsigned __int32 uint32_t
Definition: rptypes.h:49
Declares a class that represents a Rao-Blackwellized set of particles for solving the SLAM problem (T...
Used for returning entropy related information.
double z
X,Y,Z, coords.
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
Definition: TKLDParams.cpp:53
void clear()
Remove all stored pairs.
Definition: CSimpleMap.cpp:79
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
Definition: CStream.cpp:507
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092



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