Main MRPT website > C++ reference for MRPT 1.5.7
CBeaconMap.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 "maps-precomp.h" // Precomp header
11 
12 #include <mrpt/maps/CBeaconMap.h>
13 #include <mrpt/obs/CObservationBeaconRanges.h>
14 #include <mrpt/random.h>
15 #include <mrpt/utils/CFileOutputStream.h>
16 #include <mrpt/utils/CConfigFileBase.h>
17 #include <mrpt/utils/round.h> // round()
18 #include <mrpt/math/geometry.h>
19 #include <mrpt/bayes/CParticleFilterCapable.h>
20 #include <mrpt/bayes/CParticleFilter.h>
21 #include <mrpt/math/data_utils.h> // averageLogLikelihood()
22 #include <mrpt/system/os.h>
23 #include <mrpt/utils/CStream.h>
24 
25 #include <mrpt/opengl/COpenGLScene.h>
26 #include <mrpt/opengl/CSetOfObjects.h>
27 #include <mrpt/opengl/CGridPlaneXY.h>
28 #include <mrpt/opengl/stock_objects.h>
29 
30 using namespace mrpt;
31 using namespace mrpt::maps;
32 using namespace mrpt::math;
33 using namespace mrpt::obs;
34 using namespace mrpt::utils;
35 using namespace mrpt::random;
36 using namespace mrpt::poses;
37 using namespace mrpt::bayes;
38 using namespace mrpt::system;
39 using namespace std;
40 
41 // =========== Begin of Map definition ============
42 MAP_DEFINITION_REGISTER("CBeaconMap,beaconMap", mrpt::maps::CBeaconMap)
43 
44 CBeaconMap::TMapDefinition::TMapDefinition()
45 {
46 }
47 
48 void CBeaconMap::TMapDefinition::loadFromConfigFile_map_specific(const mrpt::utils::CConfigFileBase &source, const std::string &sectionNamePrefix)
49 {
50  // [<sectionNamePrefix>+"_creationOpts"]
51  //const std::string sSectCreation = sectionNamePrefix+string("_creationOpts");
52  //MRPT_LOAD_CONFIG_VAR(resolution, float, source,sSectCreation);
53 
54  insertionOpts.loadFromConfigFile(source, sectionNamePrefix+string("_insertOpts") );
55  likelihoodOpts.loadFromConfigFile(source, sectionNamePrefix+string("_likelihoodOpts") );
56 }
57 
58 void CBeaconMap::TMapDefinition::dumpToTextStream_map_specific(mrpt::utils::CStream &out) const
59 {
60  //LOADABLEOPTS_DUMP_VAR(resolution , float);
61 
62  this->insertionOpts.dumpToTextStream(out);
63  this->likelihoodOpts.dumpToTextStream(out);
64 }
65 
66 mrpt::maps::CMetricMap* CBeaconMap::internal_CreateFromMapDefinition(const mrpt::maps::TMetricMapInitializer &_def)
67 {
68  const CBeaconMap::TMapDefinition &def = *dynamic_cast<const CBeaconMap::TMapDefinition*>(&_def);
69  CBeaconMap *obj = new CBeaconMap();
70  obj->insertionOptions = def.insertionOpts;
71  obj->likelihoodOptions = def.likelihoodOpts;
72  return obj;
73 }
74 // =========== End of Map definition Block =========
75 
76 
77 IMPLEMENTS_SERIALIZABLE(CBeaconMap, CMetricMap,mrpt::maps)
78 
79 /*---------------------------------------------------------------
80  Constructor
81  ---------------------------------------------------------------*/
82 CBeaconMap::CBeaconMap() :
83  m_beacons(),
84  likelihoodOptions(),
85  insertionOptions()
86 {
87 }
88 
89 /*---------------------------------------------------------------
90  clear
91  ---------------------------------------------------------------*/
92 void CBeaconMap::internal_clear()
93 {
94  m_beacons.clear();
95 }
96 
97 /*---------------------------------------------------------------
98  getLandmarksCount
99  ---------------------------------------------------------------*/
100 size_t CBeaconMap::size() const
101 {
102  return m_beacons.size();
103 }
104 
105 /*---------------------------------------------------------------
106  Resize
107  ---------------------------------------------------------------*/
108 void CBeaconMap::resize(const size_t N)
109 {
110  m_beacons.resize(N);
111 }
112 
113 /*---------------------------------------------------------------
114  writeToStream
115  Implements the writing to a CStream capability of
116  CSerializable objects
117  ---------------------------------------------------------------*/
118 void CBeaconMap::writeToStream(mrpt::utils::CStream &out, int *version) const
119 {
120  if (version)
121  *version = 1;
122  else
123  {
124  out << genericMapParams; // v1
125 
126  // First, write the number of landmarks:
127  const uint32_t n = m_beacons.size();
128  out << n;
129  // Write all landmarks:
130  for (const_iterator it=begin();it!=end();++it)
131  out << (*it);
132 
133  }
134 }
135 
136 /*---------------------------------------------------------------
137  readFromStream
138  Implements the reading from a CStream capability of
139  CSerializable objects
140  ---------------------------------------------------------------*/
141 void CBeaconMap::readFromStream(mrpt::utils::CStream &in, int version)
142 {
143  switch(version)
144  {
145  case 0:
146  case 1:
147  {
148  if (version>=1)
149  in >> genericMapParams; // v1
150 
151  uint32_t n,i;
152 
153  // Delete previous content of map:
154  clear();
155 
156  // Load from stream:
157  // Read all landmarks:
158  in >> n;
159  m_beacons.resize(n);
160  for (i=0;i<n;i++) in >> m_beacons[i];
161 
162  } break;
163  default:
164  MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
165  };
166 }
167 
168 
169 /*---------------------------------------------------------------
170  computeObservationLikelihood
171  ---------------------------------------------------------------*/
172 double CBeaconMap::internal_computeObservationLikelihood(
173  const CObservation *obs,
174  const CPose3D &robotPose3D )
175 {
176  MRPT_START
177 
178  /* ===============================================================================================================
179  Refer to the papers:
180  - IROS 2008, "Efficient Probabilistic Range-Only SLAM",
181  http://www.mrpt.org/paper-ro-slam-with-sog/
182 
183  - ICRA 2008, "A Pure Probabilistic Approach to Range-Only SLAM",
184  http://www.mrpt.org/tutorials/slam-algorithms/rangeonly_slam/
185  =============================================================================================================== */
186 
187  if ( CLASS_ID(CObservationBeaconRanges)==obs->GetRuntimeClass() )
188  {
189  /********************************************************************
190 
191  OBSERVATION TYPE: CObservationBeaconRanges
192 
193  Lik. between "this" and "auxMap";
194 
195  ********************************************************************/
196  double ret = 0;
197  const CObservationBeaconRanges *o = static_cast<const CObservationBeaconRanges*>(obs);
198  deque<CObservationBeaconRanges::TMeasurement>::const_iterator it_obs;
199  const CBeacon *beac;
200  CPoint3D sensor3D;
201 
202  for (it_obs = o->sensedData.begin();it_obs!=o->sensedData.end();++it_obs)
203  {
204  // Look for the beacon in this map:
205  beac=getBeaconByID( it_obs->beaconID );
206 
207  if (beac!=NULL &&
208  it_obs->sensedDistance > 0 &&
209  !isNaN(it_obs->sensedDistance))
210  {
211  float sensedRange = it_obs->sensedDistance;
212 
213  // FOUND: Compute the likelihood function:
214  // -----------------------------------------------------
215  // Compute the 3D position of the sensor:
216  sensor3D = robotPose3D + it_obs->sensorLocationOnRobot;
217 
218  // Depending on the PDF type of the beacon in the map:
219  switch(beac->m_typePDF)
220  {
221  // ------------------------------
222  // PDF is MonteCarlo
223  // ------------------------------
224  case CBeacon::pdfMonteCarlo:
225  {
226  CPointPDFParticles::CParticleList::const_iterator it;
227  CVectorDouble logWeights(beac->m_locationMC.m_particles.size());
228  CVectorDouble logLiks(beac->m_locationMC.m_particles.size());
229  CVectorDouble::iterator itLW,itLL;
230 
231  for (it=beac->m_locationMC.m_particles.begin(),itLW=logWeights.begin(),itLL=logLiks.begin();it!=beac->m_locationMC.m_particles.end();++it,++itLW,++itLL)
232  {
233  float expectedRange = sensor3D.distance3DTo( it->d->x,it->d->y,it->d->z );
234  //expectedRange += float(0.1*(1-exp(-0.16*expectedRange)));
235 
236  *itLW = it->log_w; // Linear weight of this likelihood component
237  *itLL = -0.5*square((sensedRange-expectedRange)/likelihoodOptions.rangeStd);
238  //ret+= exp( -0.5*square((sensedRange-expectedRange)/likelihoodOptions.rangeStd) );
239  } // end for it
240 
241  if (logWeights.size())
242  ret+= math::averageLogLikelihood(logWeights,logLiks); // A numerically-stable method to average the likelihoods
243  }
244  break;
245  // ------------------------------
246  // PDF is Gaussian
247  // ------------------------------
248  case CBeacon::pdfGauss:
249  {
250  // Compute the Jacobian H and varZ
251  CMatrixFixedNumeric<double,1,3> H;
252  float varZ, varR = square( likelihoodOptions.rangeStd );
253  float Ax = beac->m_locationGauss.mean.x() - sensor3D.x();
254  float Ay = beac->m_locationGauss.mean.y() - sensor3D.y();
255  float Az = beac->m_locationGauss.mean.z() - sensor3D.z();
256  H(0,0) = Ax;
257  H(0,1) = Ay;
258  H(0,2) = Az;
259  float expectedRange = sensor3D.distanceTo( beac->m_locationGauss.mean );
260  H *= 1.0/expectedRange; //sqrt(Ax*Ax+Ay*Ay+Az*Az);
261 
262  varZ = H.multiply_HCHt_scalar(beac->m_locationGauss.cov);
263 
264  varZ += varR;
265 
266  // Compute the mean expected range (add bias!):
267  //expectedRange += float(0.1*(1-exp(-0.16*expectedRange)));
268 
269  // Compute the likelihood:
270  // lik \propto exp( -0.5* ( ^z - z )^2 / varZ );
271  // log_lik = -0.5* ( ^z - z )^2 / varZ
272  ret += -0.5 * square( sensedRange - expectedRange ) / varZ;
273  }
274  break;
275  // ------------------------------
276  // PDF is SOG
277  // ------------------------------
278  case CBeacon::pdfSOG:
279  {
280  CMatrixDouble13 H;
281  CVectorDouble logWeights(beac->m_locationSOG.size());
282  CVectorDouble logLiks(beac->m_locationSOG.size());
283  CVectorDouble::iterator itLW,itLL;
284  CPointPDFSOG::const_iterator it;
285  // For each Gaussian mode:
286  for (it=beac->m_locationSOG.begin(),itLW=logWeights.begin(),itLL=logLiks.begin();it!=beac->m_locationSOG.end();++it,++itLW,++itLL)
287  {
288  // Compute the Jacobian H and varZ
289  double varZ, varR = square( likelihoodOptions.rangeStd );
290  double Ax = it->val.mean.x() - sensor3D.x();
291  double Ay = it->val.mean.y() - sensor3D.y();
292  double Az = it->val.mean.z() - sensor3D.z();
293  H(0,0) = Ax;
294  H(0,1) = Ay;
295  H(0,2) = Az;
296  double expectedRange = sensor3D.distanceTo( it->val.mean );
297  H *= 1.0/expectedRange; //sqrt(Ax*Ax+Ay*Ay+Az*Az);
298 
299  varZ = H.multiply_HCHt_scalar(it->val.cov);
300  varZ += varR;
301 
302  // Compute the mean expected range (add bias!):
303  //expectedRange += float(0.1*(1-exp(-0.16*expectedRange)));
304 
305  // Compute the likelihood:
306  *itLW = it->log_w; // log-weight of this likelihood component
307  *itLL = -0.5 * square( sensedRange - expectedRange ) / varZ;
308  } // end for each mode
309 
310  // Accumulate to the overall (log) likelihood value:
311  if (logWeights.size())
312  ret += math::averageLogLikelihood(logWeights,logLiks); // log( linear_lik / sumW );
313  }
314  break;
315 
316  default:
317  THROW_EXCEPTION("Invalid beac->m_typePDF!!!");
318  };
319  }
320  else
321  {
322  // If not found, a uniform distribution:
323  if ( o->maxSensorDistance != o->minSensorDistance )
324  ret+= log(1.0/ (o->maxSensorDistance - o->minSensorDistance));
325  }
326  } // for each sensed beacon "it"
327 
328  //printf("ret: %e\n",ret);
329  MRPT_CHECK_NORMAL_NUMBER(ret);
330  return ret;
331 
332  } // end of likelihood of CObservationBeaconRanges
333  else
334  {
335  /********************************************************************
336  OBSERVATION TYPE: Unknown
337  ********************************************************************/
338  return 0;
339  }
340  MRPT_END
341 }
342 
343 /*---------------------------------------------------------------
344  insertObservation
345  ---------------------------------------------------------------*/
346 bool CBeaconMap::internal_insertObservation( const mrpt::obs::CObservation *obs, const CPose3D *robotPose)
347 {
348  MRPT_START
349 
350  CPose2D robotPose2D;
351  CPose3D robotPose3D;
352 
353  if (robotPose)
354  {
355  robotPose2D = CPose2D(*robotPose);
356  robotPose3D = (*robotPose);
357  }
358  else
359  {
360  // Default values are (0,0,0)
361  }
362 
363  if ( CLASS_ID(CObservationBeaconRanges)==obs->GetRuntimeClass() )
364  {
365  /********************************************************************
366  OBSERVATION TYPE: CObservationBeaconRanges
367  ********************************************************************/
368 
369  /* ===============================================================================================================
370  Refer to the papers:
371  - IROS 2008, "Efficient Probabilistic Range-Only SLAM",
372  http://www.mrpt.org/paper-ro-slam-with-sog/
373 
374  - ICRA 2008, "A Pure Probabilistic Approach to Range-Only SLAM",
375  http://www.mrpt.org/tutorials/slam-algorithms/rangeonly_slam/
376  =============================================================================================================== */
377 
378  // Here we fuse OR create the beacon position PDF:
379  // --------------------------------------------------------
380  const CObservationBeaconRanges *o = static_cast<const CObservationBeaconRanges*>(obs);
381 
382  for (deque<CObservationBeaconRanges::TMeasurement>::const_iterator it=o->sensedData.begin();it!=o->sensedData.end();++it)
383  {
384  CPoint3D sensorPnt( robotPose3D + it->sensorLocationOnRobot );
385  float sensedRange = it->sensedDistance;
386  unsigned int sensedID = it->beaconID;
387 
388  CBeacon *beac = getBeaconByID(sensedID);
389 
390  if (sensedRange>0) // Only sensible range values!
391  {
392  if (!beac)
393  {
394  // ======================================
395  // INSERT
396  // ======================================
397  CBeacon newBeac;
398  newBeac.m_ID = sensedID;
399 
400  if ( insertionOptions.insertAsMonteCarlo )
401  {
402  // Insert as a new set of samples:
403  // ------------------------------------------------
404 
405  newBeac.m_typePDF = CBeacon::pdfMonteCarlo;
406 
407  size_t numParts = round(insertionOptions.MC_numSamplesPerMeter * sensedRange);
408  ASSERT_(insertionOptions.minElevation_deg<=insertionOptions.maxElevation_deg)
409  double minA = DEG2RAD(insertionOptions.minElevation_deg);
410  double maxA = DEG2RAD(insertionOptions.maxElevation_deg);
411  newBeac.m_locationMC.setSize(numParts);
412  for ( CPointPDFParticles::CParticleList::iterator itP=newBeac.m_locationMC.m_particles.begin();itP!=newBeac.m_locationMC.m_particles.end();++itP)
413  {
414  double th = randomGenerator.drawUniform(-M_PI,M_PI);
415  double el = randomGenerator.drawUniform(minA,maxA);
416  double R = randomGenerator.drawGaussian1D(sensedRange , likelihoodOptions.rangeStd );
417  itP->d->x = sensorPnt.x() + R*cos(th)*cos(el);
418  itP->d->y = sensorPnt.y() + R*sin(th)*cos(el);
419  itP->d->z = sensorPnt.z() + R*sin(el);
420  } // end for itP
421  }
422  else
423  {
424  // Insert as a Sum of Gaussians:
425  // ------------------------------------------------
426  newBeac.m_typePDF = CBeacon::pdfSOG;
427  CBeacon::generateRingSOG(
428  sensedRange, // Sensed range
429  newBeac.m_locationSOG, // Output SOG
430  this, // My CBeaconMap, for options.
431  sensorPnt // Sensor point
432  );
433  }
434 
435  // and insert it:
436  m_beacons.push_back( newBeac );
437 
438  } // end insert
439  else
440  {
441  // ======================================
442  // FUSE
443  // ======================================
444  switch(beac->m_typePDF)
445  {
446  // ------------------------------
447  // FUSE: PDF is MonteCarlo
448  // ------------------------------
449  case CBeacon::pdfMonteCarlo:
450  {
451  double maxW = -1e308, sumW=0;
452  // Update weights:
453  // --------------------
454  for (CPointPDFParticles::CParticleList::iterator it=beac->m_locationMC.m_particles.begin();it!=beac->m_locationMC.m_particles.end();++it)
455  {
456  float expectedRange = sensorPnt.distance3DTo( it->d->x,it->d->y,it->d->z );
457  // Add bias:
458  //expectedRange += float(0.1*(1-exp(-0.16*expectedRange)));
459  it->log_w += -0.5*square((sensedRange-expectedRange)/likelihoodOptions.rangeStd);
460  maxW=max(it->log_w,maxW);
461  sumW+=exp(it->log_w);
462  } // end for it
463 
464  // Perform resampling (SIR filter) or not (simply accumulate weights)??
465  // ------------------------------------------------------------------------
466  if (insertionOptions.MC_performResampling)
467  {
468  // Yes, perform an auxiliary PF SIR here:
469  // ---------------------------------------------
470  if (beac->m_locationMC.ESS() < 0.5)
471  {
472  // We must resample:
473  // Make a list with the log weights:
474  vector<double> log_ws;
475  vector<size_t> indxs;
476  beac->m_locationMC.getWeights( log_ws );
477 
478  // And compute the resampled indexes:
479  CParticleFilterCapable::computeResampling(
480  CParticleFilter::prSystematic,
481  log_ws,
482  indxs );
483 
484  // Replace with the new samples:
485  beac->m_locationMC.performSubstitution( indxs );
486 
487  // Determine if this is a 2D beacon map:
488  bool is2D = (insertionOptions.minElevation_deg==insertionOptions.maxElevation_deg);
489  float noiseStd = insertionOptions.MC_afterResamplingNoise;
490 
491  // AND, add a small noise:
492  CPointPDFParticles::CParticleList::iterator itSample;
493  for (itSample=beac->m_locationMC.m_particles.begin();itSample!=beac->m_locationMC.m_particles.end();++itSample)
494  {
495  itSample->d->x += randomGenerator.drawGaussian1D( 0,noiseStd );
496  itSample->d->y += randomGenerator.drawGaussian1D( 0,noiseStd );
497  if (!is2D)
498  itSample->d->z += randomGenerator.drawGaussian1D( 0,noiseStd );
499  }
500 
501  }
502  } // end "do resample"
503  else
504  {
505  // Do not resample:
506  // ---------------------------------------------
507 
508  // Remove very very very unlikely particles:
509  // -------------------------------------------
510  for (CPointPDFParticles::CParticleList::iterator it=beac->m_locationMC.m_particles.begin();it!=beac->m_locationMC.m_particles.end(); )
511  {
512  if ( it->log_w < (maxW-insertionOptions.MC_thresholdNegligible) )
513  {
514  it->d.reset();
515  it = beac->m_locationMC.m_particles.erase( it );
516  }
517  else ++it;
518  }
519  } // end "do not resample"
520 
521  // Normalize weights:
522  // log_w = log( exp(log_w)/sumW ) ->
523  // log_w -= log(sumW);
524  // -----------------------------------------
525  sumW=log(sumW);
526  for (CPointPDFParticles::CParticleList::iterator it=beac->m_locationMC.m_particles.begin();it!=beac->m_locationMC.m_particles.end();++it)
527  it->log_w -= sumW;
528 
529  // Is the moment to turn into a Gaussian??
530  // -------------------------------------------
531  CPoint3D MEAN;
532  CMatrixDouble33 COV;
533  beac->m_locationMC.getCovarianceAndMean(COV,MEAN);
534 
535  double D1 = sqrt(COV(0,0));
536  double D2 = sqrt(COV(1,1));
537  double D3 = sqrt(COV(2,2));
538 
539  double mxVar = max3( D1, D2, D3 );
540 
541  if (mxVar < insertionOptions.MC_maxStdToGauss )
542  {
543  // Collapse into Gaussian:
544  beac->m_locationMC.clear(); // Erase prev. samples
545 
546  // Assure a non-null covariance!
547  CMatrixDouble COV2 = CMatrixDouble(COV);
548  COV2.setSize(2,2);
549  if (COV2.det()==0)
550  {
551  COV.setIdentity();
552  COV*= square( 0.01f );
553  if (insertionOptions.minElevation_deg == insertionOptions.maxElevation_deg )
554  COV(2,2) = 0; // We are in a 2D map:
555  }
556 
557  beac->m_typePDF = CBeacon::pdfGauss; // Pass to gaussian.
558  beac->m_locationGauss.mean = MEAN;
559  beac->m_locationGauss.cov = COV;
560  }
561  }
562  break;
563 
564  // ------------------------------
565  // FUSE: PDF is Gaussian:
566  // ------------------------------
567  case CBeacon::pdfGauss:
568  {
569  // Compute the mean expected range:
570  float expectedRange = sensorPnt.distanceTo( beac->m_locationGauss.mean );
571  float varR = square( likelihoodOptions.rangeStd );
572  //bool useEKF_or_KF = true;
573 
574  //if (useEKF_or_KF)
575  {
576  // EKF method:
577  // ---------------------
578  // Add bias:
579  //expectedRange += float(0.1*(1-exp(-0.16*expectedRange)));
580 
581  // An EKF for updating the Gaussian:
582  float y = sensedRange - expectedRange;
583 
584  // Compute the Jacobian H and varZ
585  CMatrixDouble13 H;
586  double varZ;
587  double Ax = (beac->m_locationGauss.mean.x() - sensorPnt.x());
588  double Ay = (beac->m_locationGauss.mean.y() - sensorPnt.y());
589  double Az = (beac->m_locationGauss.mean.z() - sensorPnt.z());
590  H(0,0) = Ax; H(0,1) = Ay; H(0,2) = Az;
591  H *= 1.0/expectedRange; //sqrt(Ax*Ax+Ay*Ay+Az*Az);
592  varZ = H.multiply_HCHt_scalar(beac->m_locationGauss.cov);
593  varZ += varR;
594 
595  CMatrixDouble31 K;
596  K.multiply_ABt( beac->m_locationGauss.cov, H );
597  K *= 1.0/varZ;
598 
599  // Update stage of the EKF:
600  beac->m_locationGauss.mean.x_incr( K(0,0) * y );
601  beac->m_locationGauss.mean.y_incr(K(1,0) * y );
602  beac->m_locationGauss.mean.z_incr( K(2,0) * y );
603 
604  beac->m_locationGauss.cov = (Eigen::Matrix<double,3,3>::Identity() - K*H) * beac->m_locationGauss.cov;
605  //beac->m_locationGauss.cov.force_symmetry();
606  }
607  }
608  break;
609  // ------------------------------
610  // FUSE: PDF is SOG
611  // ------------------------------
612  case CBeacon::pdfSOG:
613  {
614  // Compute the mean expected range for this mode:
615  float varR = square( likelihoodOptions.rangeStd );
616 
617  // For each Gaussian mode:
618  // 1) Update its weight (using the likelihood of the observation linearized at the mean)
619  // 2) Update its mean/cov (as in the simple EKF)
620  CPointPDFSOG::iterator it;
621  double max_w = -1e9;
622  for (it=beac->m_locationSOG.begin();it!=beac->m_locationSOG.end();++it)
623  {
624  double expectedRange = sensorPnt.distanceTo( it->val.mean );
625 
626  // An EKF for updating the Gaussian:
627  double y = sensedRange - expectedRange;
628 
629  // Compute the Jacobian H and varZ
630  CMatrixDouble13 H;
631  double varZ;
632  double Ax = ( it->val.mean.x() - sensorPnt.x());
633  double Ay = ( it->val.mean.y() - sensorPnt.y());
634  double Az = ( it->val.mean.z() - sensorPnt.z());
635  H(0,0) = Ax; H(0,1) = Ay; H(0,2) = Az;
636  H *= 1.0/expectedRange; //sqrt(Ax*Ax+Ay*Ay+Az*Az);
637  varZ = H.multiply_HCHt_scalar( it->val.cov );
638  varZ += varR;
639  CMatrixDouble31 K;
640  K.multiply( it->val.cov, H.transpose());
641  K *= 1.0/varZ;
642 
643  // Update stage of the EKF:
644  it->val.mean.x_incr( K(0,0) * y );
645  it->val.mean.y_incr( K(1,0) * y );
646  it->val.mean.z_incr( K(2,0) * y );
647 
648  it->val.cov = (Eigen::Matrix<double,3,3>::Identity() - K*H) * it->val.cov;
649  //it->val.cov.force_symmetry();
650 
651  // Update the weight of this mode:
652  // ----------------------------------
653  it->log_w += -0.5 * square( y ) / varZ;
654 
655  max_w = max(max_w,it->log_w); // keep the maximum mode weight
656  } // end for each mode
657 
658  // Remove modes with negligible weights:
659  // -----------------------------------------------------------
660  for (it=beac->m_locationSOG.begin();it!=beac->m_locationSOG.end(); )
661  {
662  if (max_w - it->log_w > insertionOptions.SOG_thresholdNegligible )
663  {
664  // Remove the mode:
665  it = beac->m_locationSOG.erase( it );
666  }
667  else ++it;
668  }
669 
670  //printf("ESS: %f\n",beac->m_locationSOG.ESS());
671 
672  // Normalize the weights:
673  beac->m_locationSOG.normalizeWeights();
674 
675  // Should we pass this beacon to a single Gaussian mode?
676  // -----------------------------------------------------------
677  CPoint3D curMean;
678  CMatrixDouble33 curCov;
679  beac->m_locationSOG.getCovarianceAndMean(curCov,curMean);
680 
681  double D1 = sqrt(curCov(0,0));
682  double D2 = sqrt(curCov(1,1));
683  double D3 = sqrt(curCov(2,2));
684  float maxDiag = max3(D1,D2,D3);
685 
686  if (maxDiag<0.10f)
687  {
688  // Yes, transform:
689  beac->m_locationGauss.mean = curMean;
690  beac->m_locationGauss.cov = curCov;
691  beac->m_typePDF = CBeacon::pdfGauss;
692  // Done!
693  }
694  }
695  break;
696  default:
697  THROW_EXCEPTION("Invalid beac->m_typePDF!!!");
698  };
699 
700  } // end fuse
701  } // end if range makes sense
702  } // end for each observation
703 
704  // DONE!!
705  // Observation was successfully inserted into the map
706  return true;
707  }
708  else
709  {
710  return false;
711  }
712 
713  MRPT_END
714 }
715 
716 /*---------------------------------------------------------------
717  determineMatching2D
718  ---------------------------------------------------------------*/
719 void CBeaconMap::determineMatching2D(
720  const mrpt::maps::CMetricMap * otherMap,
721  const CPose2D & otherMapPose,
722  TMatchingPairList & correspondences,
723  const TMatchingParams & params,
724  TMatchingExtraResults & extraResults ) const
725 {
726  MRPT_UNUSED_PARAM(params);
727  MRPT_START
728  extraResults = TMatchingExtraResults();
729 
730  CBeaconMap auxMap;
731  CPose3D otherMapPose3D(otherMapPose);
732 
733  // Check the other map class:
734  ASSERT_( otherMap->GetRuntimeClass() == CLASS_ID(CBeaconMap) );
735  const CBeaconMap *otherMap2 = static_cast<const CBeaconMap*>( otherMap );
736  vector<bool> otherCorrespondences;
737 
738  // Coordinates change:
739  auxMap.changeCoordinatesReference( otherMapPose3D, otherMap2 );
740 
741  // Use the 3D matching method:
742  computeMatchingWith3DLandmarks( otherMap2,
743  correspondences,
744  extraResults.correspondencesRatio,
745  otherCorrespondences );
746 
747  MRPT_END
748 
749 }
750 
751 /*---------------------------------------------------------------
752  changeCoordinatesReference
753  ---------------------------------------------------------------*/
754 void CBeaconMap::changeCoordinatesReference( const CPose3D &newOrg )
755 {
756  // Change the reference of each individual beacon:
757  for (iterator lm=m_beacons.begin();lm!=m_beacons.end();++lm)
758  lm->changeCoordinatesReference(newOrg);
759 }
760 
761 /*---------------------------------------------------------------
762  changeCoordinatesReference
763  ---------------------------------------------------------------*/
764 void CBeaconMap::changeCoordinatesReference( const CPose3D &newOrg, const mrpt::maps::CBeaconMap *otherMap )
765 {
766  // In this object we cannot apply any special speed-up: Just copy and change coordinates:
767  (*this) = *otherMap;
768  changeCoordinatesReference(newOrg);
769 }
770 
771 
772 /*---------------------------------------------------------------
773  computeMatchingWith3DLandmarks
774  ---------------------------------------------------------------*/
775 void CBeaconMap::computeMatchingWith3DLandmarks(
776  const mrpt::maps::CBeaconMap *anotherMap,
777  TMatchingPairList &correspondences,
778  float &correspondencesRatio,
779  vector<bool> &otherCorrespondences) const
780 {
781  MRPT_START
782 
783  TSequenceBeacons::const_iterator thisIt,otherIt;
784  size_t nThis,nOther;
785  unsigned int j,k;
786  TMatchingPair match;
787  CPointPDFGaussian pointPDF_k, pointPDF_j;
788  vector<bool> thisLandmarkAssigned;
789 
790  // Get the number of landmarks:
791  nThis = m_beacons.size();
792  nOther = anotherMap->m_beacons.size();
793 
794  // Initially no LM has a correspondence:
795  thisLandmarkAssigned.resize( nThis, false );
796 
797  // Initially, set all landmarks without correspondences:
798  correspondences.clear();
799  otherCorrespondences.clear();
800  otherCorrespondences.resize( nOther, false );
801  correspondencesRatio = 0;
802 
803  for (k=0,otherIt=anotherMap->m_beacons.begin();otherIt!=anotherMap->m_beacons.end();++otherIt,++k)
804  {
805  for (j=0,thisIt=m_beacons.begin();thisIt!=m_beacons.end();++thisIt,++j)
806  {
807  // Is it a correspondence?
808  if ( (otherIt)->m_ID == (thisIt)->m_ID )
809  {
810  // If a previous correspondence for this LM was found, discard this one!
811  if ( !thisLandmarkAssigned[ j ] )
812  {
813  thisLandmarkAssigned[ j ] = true;
814 
815  // OK: A correspondence found!!
816  otherCorrespondences[ k ] = true;
817 
818  match.this_idx = j;
819 
820  CPoint3D mean_j = m_beacons[j].getMeanVal();
821 
822  match.this_x = mean_j.x();
823  match.this_y = mean_j.y();
824  match.this_z = mean_j.z();
825 
826  CPoint3D mean_k = anotherMap->m_beacons[k].getMeanVal();
827  match.other_idx = k;
828  match.other_x = mean_k.x();
829  match.other_y = mean_k.y();
830  match.other_z = mean_k.z();
831 
832  correspondences.push_back( match );
833  }
834  }
835 
836  } // end of "otherIt" is SIFT
837 
838  } // end of other it., k
839 
840  // Compute the corrs ratio:
841  correspondencesRatio = 2.0f * correspondences.size() / static_cast<float>( nThis + nOther);
842 
843  MRPT_END
844 }
845 
846 /*---------------------------------------------------------------
847  saveToMATLABScript3D
848  ---------------------------------------------------------------*/
849 bool CBeaconMap::saveToMATLABScript3D(
850  const string & file,
851  const char *style,
852  float confInterval ) const
853 {
854  MRPT_UNUSED_PARAM(style);
855  MRPT_UNUSED_PARAM(confInterval);
856 
857  FILE *f= os::fopen(file.c_str(),"wt");
858  if (!f)
859  return false;
860 
861  // Header:
862  os::fprintf(f,"%%-------------------------------------------------------\n");
863  os::fprintf(f,"%% File automatically generated using the MRPT method:\n");
864  os::fprintf(f,"%% 'CBeaconMap::saveToMATLABScript3D'\n");
865  os::fprintf(f,"%%\n");
866  os::fprintf(f,"%% ~ MRPT ~\n");
867  os::fprintf(f,"%% Jose Luis Blanco Claraco, University of Malaga @ 2006\n");
868  os::fprintf(f,"%% http://www.isa.uma.es/ \n");
869  os::fprintf(f,"%%-------------------------------------------------------\n\n");
870 
871  // Main code:
872  os::fprintf(f,"hold on;\n\n");
873  utils::CStringList strs;
874  string s;
875 
876  for (const_iterator it=m_beacons.begin();it!=m_beacons.end();++it)
877  {
878  it->getAsMatlabDrawCommands( strs );
879  strs.getText(s);
880  os::fprintf(f,"%s",s.c_str());
881  }
882 
883  os::fprintf(f,"axis equal;grid on;");
884 
885  os::fclose(f);
886  return true;
887 }
888 
889 /*---------------------------------------------------------------
890  TLikelihoodOptions
891  ---------------------------------------------------------------*/
892 CBeaconMap::TLikelihoodOptions::TLikelihoodOptions() :
893  rangeStd (0.08f )
894 {
895 }
896 
897 /*---------------------------------------------------------------
898  dumpToTextStream
899  ---------------------------------------------------------------*/
900 void CBeaconMap::TLikelihoodOptions::dumpToTextStream(mrpt::utils::CStream &out) const
901 {
902  out.printf("\n----------- [CBeaconMap::TLikelihoodOptions] ------------ \n\n");
903 
904  out.printf("rangeStd = %f\n",rangeStd);
905 
906  out.printf("\n");
907 }
908 
909 /*---------------------------------------------------------------
910  loadFromConfigFile
911  ---------------------------------------------------------------*/
912 void CBeaconMap::TLikelihoodOptions::loadFromConfigFile(
913  const mrpt::utils::CConfigFileBase &iniFile,
914  const string &section)
915 {
916  rangeStd = iniFile.read_float(section.c_str(),"rangeStd",rangeStd);
917 }
918 
919 /*---------------------------------------------------------------
920  TInsertionOptions
921  ---------------------------------------------------------------*/
922 CBeaconMap::TInsertionOptions::TInsertionOptions() :
923  insertAsMonteCarlo ( true ),
924  maxElevation_deg ( 0),
925  minElevation_deg ( 0 ),
926  MC_numSamplesPerMeter ( 1000 ),
927  MC_maxStdToGauss ( 0.4f ),
928  MC_thresholdNegligible ( 5 ),
929  MC_performResampling ( false ),
930  MC_afterResamplingNoise ( 0.01f ),
931  SOG_thresholdNegligible ( 20.0f ),
932  SOG_maxDistBetweenGaussians ( 1.0f ),
933  SOG_separationConstant ( 3.0f )
934 {
935 }
936 
937 /*---------------------------------------------------------------
938  dumpToTextStream
939  ---------------------------------------------------------------*/
940 void CBeaconMap::TInsertionOptions::dumpToTextStream(mrpt::utils::CStream &out) const
941 {
942  out.printf("\n----------- [CBeaconMap::TInsertionOptions] ------------ \n\n");
943 
944  out.printf("insertAsMonteCarlo = %c\n",insertAsMonteCarlo ? 'Y':'N');
945  out.printf("minElevation_deg = %.03f\n",minElevation_deg);
946  out.printf("maxElevation_deg = %.03f\n",maxElevation_deg);
947  out.printf("MC_numSamplesPerMeter = %d\n",MC_numSamplesPerMeter);
948  out.printf("MC_maxStdToGauss = %.03f\n",MC_maxStdToGauss);
949  out.printf("MC_thresholdNegligible = %.03f\n",MC_thresholdNegligible);
950  out.printf("MC_performResampling = %c\n",MC_performResampling ? 'Y':'N');
951  out.printf("MC_afterResamplingNoise = %.03f\n",MC_afterResamplingNoise);
952  out.printf("SOG_thresholdNegligible = %.03f\n",SOG_thresholdNegligible);
953  out.printf("SOG_maxDistBetweenGaussians = %.03f\n",SOG_maxDistBetweenGaussians);
954  out.printf("SOG_separationConstant = %.03f\n",SOG_separationConstant);
955 
956 
957  out.printf("\n");
958 }
959 
960 /*---------------------------------------------------------------
961  loadFromConfigFile
962  ---------------------------------------------------------------*/
963 void CBeaconMap::TInsertionOptions::loadFromConfigFile(
964  const mrpt::utils::CConfigFileBase &iniFile,
965  const string &section)
966 {
967  MRPT_LOAD_CONFIG_VAR(insertAsMonteCarlo,bool, iniFile,section.c_str());
968  MRPT_LOAD_CONFIG_VAR(maxElevation_deg,float, iniFile,section.c_str());
969  MRPT_LOAD_CONFIG_VAR(minElevation_deg,float, iniFile,section.c_str());
970  MRPT_LOAD_CONFIG_VAR(MC_numSamplesPerMeter,int, iniFile,section.c_str());
971  MRPT_LOAD_CONFIG_VAR(MC_maxStdToGauss,float, iniFile,section.c_str());
972  MRPT_LOAD_CONFIG_VAR(MC_thresholdNegligible,float, iniFile,section.c_str());
973  MRPT_LOAD_CONFIG_VAR(MC_performResampling,bool, iniFile,section.c_str());
974  MRPT_LOAD_CONFIG_VAR(MC_afterResamplingNoise,float, iniFile,section.c_str());
975  MRPT_LOAD_CONFIG_VAR(SOG_thresholdNegligible,float, iniFile,section.c_str());
976  MRPT_LOAD_CONFIG_VAR(SOG_maxDistBetweenGaussians,float, iniFile,section.c_str());
977  MRPT_LOAD_CONFIG_VAR(SOG_separationConstant,float, iniFile,section.c_str());
978 
979 }
980 
981 /*---------------------------------------------------------------
982  isEmpty
983  ---------------------------------------------------------------*/
984 bool CBeaconMap::isEmpty() const
985 {
986  return size()==0;
987 }
988 
989 /*---------------------------------------------------------------
990  simulateBeaconReadings
991  ---------------------------------------------------------------*/
992 void CBeaconMap::simulateBeaconReadings(
993  const CPose3D &in_robotPose,
994  const CPoint3D &in_sensorLocationOnRobot,
995  CObservationBeaconRanges &out_Observations ) const
996 {
997  TSequenceBeacons::const_iterator it;
998  mrpt::obs::CObservationBeaconRanges::TMeasurement newMeas;
999  CPoint3D point3D,beacon3D;
1000  CPointPDFGaussian beaconPDF;
1001 
1002  // Compute the 3D position of the sensor:
1003  point3D = in_robotPose + in_sensorLocationOnRobot;
1004 
1005  // Clear output data:
1006  out_Observations.sensedData.clear();
1007 
1008  // For each BEACON landmark in the map:
1009  for (it=m_beacons.begin();it!=m_beacons.end();++it)
1010  {
1011  it->getMean(beacon3D);
1012 
1013  float range = point3D.distanceTo( beacon3D );
1014 
1015  if (range < out_Observations.maxSensorDistance && range > out_Observations.minSensorDistance)
1016  {
1017  // Add noise:
1018  range += randomGenerator.drawGaussian1D(0, out_Observations.stdError );
1019 
1020  // Fill out:
1021  newMeas.beaconID = it->m_ID;
1022  newMeas.sensorLocationOnRobot = in_sensorLocationOnRobot;
1023  newMeas.sensedDistance = range;
1024 
1025  // Insert:
1026  out_Observations.sensedData.push_back( newMeas );
1027  }
1028  } // end for it
1029  // Done!
1030 }
1031 
1032 /*---------------------------------------------------------------
1033  saveMetricMapRepresentationToFile
1034  ---------------------------------------------------------------*/
1035 void CBeaconMap::saveMetricMapRepresentationToFile( const string &filNamePrefix ) const
1036 {
1037  MRPT_START
1038 
1039  // Matlab:
1040  string fil1( filNamePrefix + string("_3D.m") );
1041  saveToMATLABScript3D( fil1 );
1042 
1043  // 3D Scene:
1044  opengl::COpenGLScene scene;
1045  opengl::CSetOfObjectsPtr obj3D = opengl::CSetOfObjects::Create();
1046 
1047  getAs3DObject( obj3D );
1048  opengl::CGridPlaneXYPtr objGround = opengl::CGridPlaneXY::Create(-100,100,-100,100,0,1);
1049 
1050  scene.insert(obj3D);
1051  scene.insert(objGround);
1052 
1053  string fil2( filNamePrefix + string("_3D.3Dscene") );
1054  CFileOutputStream f(fil2.c_str());
1055  f << scene;
1056 
1057  // Textual representation:
1058  string fil3( filNamePrefix + string("_covs.txt") );
1059  saveToTextFile( fil3 );
1060 
1061  // Total number of particles / modes:
1062  string fil4( filNamePrefix + string("_population.txt") );
1063  {
1064  FILE *f = os::fopen(fil4.c_str(),"wt");
1065  if (f)
1066  {
1067  size_t nParts = 0, nGaussians = 0;
1068 
1069  for (TSequenceBeacons::const_iterator it=m_beacons.begin();it!=m_beacons.end();++it)
1070  {
1071  switch (it->m_typePDF)
1072  {
1073  case CBeacon::pdfMonteCarlo:
1074  nParts += it->m_locationMC.size();
1075  break;
1076  case CBeacon::pdfSOG:
1077  nGaussians += it->m_locationSOG.size();
1078  break;
1079  case CBeacon::pdfGauss:
1080  nGaussians ++;
1081  break;
1082  };
1083  }
1084 
1085  fprintf(f,"%u %u",static_cast<unsigned>(nParts), static_cast<unsigned>(nGaussians) );
1086  os::fclose(f);
1087  }
1088  }
1089 
1090 
1091  MRPT_END
1092 }
1093 
1094 /*---------------------------------------------------------------
1095  getAs3DObject
1096  ---------------------------------------------------------------*/
1097 void CBeaconMap::getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const
1098 {
1099  MRPT_START
1100 
1101  if (!genericMapParams.enableSaveAs3DObject) return;
1102 
1103  // ------------------------------------------------
1104  // Add the XYZ corner for the current area:
1105  // ------------------------------------------------
1106  outObj->insert( opengl::stock_objects::CornerXYZ() );
1107 
1108  // Save 3D ellipsoids or whatever representation:
1109  for (const_iterator it=m_beacons.begin();it!=m_beacons.end();++it)
1110  it->getAs3DObject( outObj );
1111 
1112  MRPT_END
1113 }
1114 
1115 /*---------------------------------------------------------------
1116  Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose"
1117  * In the case of a multi-metric map, this returns the average between the maps. This method always return 0 for grid maps.
1118  * \param otherMap [IN] The other map to compute the matching with.
1119  * \param otherMapPose [IN] The 6D pose of the other map as seen from "this".
1120  * \param maxDistForCorr [IN] The minimum distance between 2 non-probabilistic map elements for counting them as a correspondence.
1121  * \param maxMahaDistForCorr [IN] The minimum Mahalanobis distance between 2 probabilistic map elements for counting them as a correspondence.
1122  *
1123  * \return The matching ratio [0,1]
1124  * \sa computeMatchingWith2D
1125  ----------------------------------------------------------------*/
1126 float CBeaconMap::compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap2, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const
1127 {
1128  MRPT_START
1129 
1130  // Compare to a similar map only:
1131  const CBeaconMap *otherMap = NULL;
1132 
1133  if ( otherMap2->GetRuntimeClass() == CLASS_ID(CBeaconMap) )
1134  otherMap = static_cast<const CBeaconMap*>( otherMap2);
1135 
1136  if (!otherMap) return 0;
1137 
1138  TMatchingPairList matchList;
1139  vector<bool> otherCorrespondences;
1140  float out_corrsRatio;
1141 
1142  CBeaconMap modMap;
1143 
1144  modMap.changeCoordinatesReference( otherMapPose, otherMap );
1145 
1146  computeMatchingWith3DLandmarks(
1147  &modMap,
1148  matchList,
1149  out_corrsRatio,
1150  otherCorrespondences );
1151 
1152  return out_corrsRatio;
1153 
1154  MRPT_END
1155 }
1156 
1157 
1158 /*---------------------------------------------------------------
1159  getBeaconByID
1160  ---------------------------------------------------------------*/
1161 const CBeacon * CBeaconMap::getBeaconByID( CBeacon::TBeaconID id ) const
1162 {
1163  for (const_iterator it=m_beacons.begin();it!=m_beacons.end();++it)
1164  if (it->m_ID==id)
1165  return &(*it);
1166  return NULL;
1167 }
1168 
1169 /*---------------------------------------------------------------
1170  getBeaconByID
1171  ---------------------------------------------------------------*/
1172 CBeacon * CBeaconMap::getBeaconByID( CBeacon::TBeaconID id )
1173 {
1174  for (iterator it=m_beacons.begin();it!=m_beacons.end();++it)
1175  if (it->m_ID==id)
1176  return &(*it);
1177  return NULL;
1178 }
1179 
1180 /*---------------------------------------------------------------
1181  saveToTextFile
1182 - VX VY VZ: Variances of each dimension (C11, C22, C33)
1183 - DET2D DET3D: Determinant of the 2D and 3D covariance matrixes.
1184 - C12, C13, C23: Cross covariances
1185  ---------------------------------------------------------------*/
1186 void CBeaconMap::saveToTextFile(const string &fil) const
1187 {
1188  MRPT_START
1189  FILE *f = os::fopen(fil.c_str(),"wt");
1190  ASSERT_(f!=NULL);
1191 
1192  CPoint3D p;
1193  CMatrixDouble33 C;
1194 
1195  for (const_iterator it=m_beacons.begin();it!=m_beacons.end();++it)
1196  {
1197  it->getCovarianceAndMean(C,p);
1198 
1199  float D3 = C.det();
1200  float D2 = C(0,0)*C(1,1) - square( C(0,1) );
1201  os::fprintf(f,"%i %f %f %f %e %e %e %e %e %e %e %e\n",
1202  static_cast<int>(it->m_ID),
1203  p.x(),p.y(),p.z(),
1204  C(0,0),C(1,1),C(2,2),
1205  D2,D3,
1206  C(0,1),C(1,2),C(1,2)
1207  );
1208  }
1209 
1210  os::fclose(f);
1211  MRPT_END
1212 }
MRPT_LOAD_CONFIG_VAR
#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...
Definition: CConfigFileBase.h:190
R
const float R
Definition: CKinematicChain.cpp:141
CLASS_ID
#define CLASS_ID(class_name)
Access to runtime class ID for a defined class name.
Definition: CObject.h:82
IMPLEMENTS_SERIALIZABLE
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
Definition: CSerializable.h:187
MAP_DEFINITION_REGISTER
#define MAP_DEFINITION_REGISTER(_CLASSNAME_STRINGS, _CLASSNAME_WITH_NS)
Registers one map class into TMetricMapInitializer factory.
Definition: TMetricMapTypesRegistry.h:69
M_PI
#define M_PI
Definition: bits.h:78
DEG2RAD
#define DEG2RAD
Definition: bits.h:99
mrpt::bayes::CParticleFilterData::m_particles
CParticleList m_particles
The array of particles.
Definition: CParticleFilterData.h:177
mrpt::bayes::CParticleFilterData::getWeights
void getWeights(std::vector< double > &out_logWeights) const
Returns a vector with the sequence of the logaritmic weights of all the samples.
Definition: CParticleFilterData.h:227
mrpt::maps::CBeacon
The class for storing individual "beacon landmarks" under a variety of 3D position PDF distributions.
Definition: maps/CBeacon.h:41
mrpt::maps::CBeacon::m_locationGauss
mrpt::poses::CPointPDFGaussian m_locationGauss
The individual PDF, if m_typePDF=pdfGauss (publicly accesible for ease of use, but the CPointPDF inte...
Definition: maps/CBeacon.h:62
mrpt::maps::CBeacon::m_typePDF
TTypePDF m_typePDF
Which one of the different 3D point PDF is currently used in this object: montecarlo,...
Definition: maps/CBeacon.h:57
mrpt::maps::CBeacon::TBeaconID
int64_t TBeaconID
The type for the IDs of landmarks.
Definition: maps/CBeacon.h:48
mrpt::maps::CBeacon::generateRingSOG
static void generateRingSOG(const float &sensedRange, mrpt::poses::CPointPDFSOG &outPDF, const CBeaconMap *myBeaconMap, const mrpt::poses::CPoint3D &sensorPnt, const mrpt::math::CMatrixDouble33 *covarianceCompositionToAdd=NULL, bool clearPreviousContentsOutPDF=true, const mrpt::poses::CPoint3D &centerPoint=mrpt::poses::CPoint3D(0, 0, 0), const float &maxDistanceFromCenter=0)
This static method returns a SOG with ring-shape (or as a 3D sphere) that can be used to initialize a...
Definition: CBeacon.cpp:432
mrpt::maps::CBeacon::m_locationSOG
mrpt::poses::CPointPDFSOG m_locationSOG
The individual PDF, if m_typePDF=pdfSOG (publicly accesible for ease of use, but the CPointPDF interf...
Definition: maps/CBeacon.h:64
mrpt::maps::CBeacon::m_ID
TBeaconID m_ID
An ID for the landmark (see details next...) This ID was introduced in the version 3 of this class (2...
Definition: maps/CBeacon.h:75
mrpt::maps::CBeacon::m_locationMC
mrpt::poses::CPointPDFParticles m_locationMC
The individual PDF, if m_typePDF=pdfMonteCarlo (publicly accesible for ease of use,...
Definition: maps/CBeacon.h:60
mrpt::maps::CBeaconMap
A class for storing a map of 3D probabilistic beacons, using a Montecarlo, Gaussian,...
Definition: maps/CBeaconMap.h:45
mrpt::maps::CBeaconMap::compute3DMatchingRatio
float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const MRPT_OVERRIDE
Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map,...
Definition: CBeaconMap.cpp:1126
mrpt::maps::CBeaconMap::getAs3DObject
void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const MRPT_OVERRIDE
Returns a 3D object representing the map.
Definition: CBeaconMap.cpp:1097
mrpt::maps::CBeaconMap::saveMetricMapRepresentationToFile
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const MRPT_OVERRIDE
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >,...
Definition: CBeaconMap.cpp:1035
mrpt::maps::CBeaconMap::determineMatching2D
virtual void determineMatching2D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose2D &otherMapPose, mrpt::utils::TMatchingPairList &correspondences, const TMatchingParams &params, TMatchingExtraResults &extraResults) const MRPT_OVERRIDE
Computes the matching between this and another 2D point map, which includes finding:
Definition: CBeaconMap.cpp:719
mrpt::maps::CBeaconMap::saveToTextFile
void saveToTextFile(const std::string &fil) const
Save a text file with a row per beacon, containing this 11 elements:
Definition: CBeaconMap.cpp:1186
mrpt::maps::CBeaconMap::getBeaconByID
const CBeacon * getBeaconByID(CBeacon::TBeaconID id) const
Returns a pointer to the beacon with the given ID, or NULL if it does not exist.
Definition: CBeaconMap.cpp:1161
mrpt::maps::CBeaconMap::internal_CreateFromMapDefinition
static mrpt::maps::CMetricMap * internal_CreateFromMapDefinition(const mrpt::maps::TMetricMapInitializer &def)
Definition: CBeaconMap.cpp:66
mrpt::maps::CBeaconMap::resize
void resize(const size_t N)
Resize the number of SOG modes.
Definition: CBeaconMap.cpp:108
mrpt::maps::CBeaconMap::writeToStream
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
Definition: CBeaconMap.cpp:118
mrpt::maps::CBeaconMap::simulateBeaconReadings
void simulateBeaconReadings(const mrpt::poses::CPose3D &in_robotPose, const mrpt::poses::CPoint3D &in_sensorLocationOnRobot, mrpt::obs::CObservationBeaconRanges &out_Observations) const
Simulates a reading toward each of the beacons in the landmarks map, if any.
Definition: CBeaconMap.cpp:992
mrpt::maps::CBeaconMap::const_iterator
std::deque< CBeacon >::const_iterator const_iterator
Definition: maps/CBeaconMap.h:52
mrpt::maps::CBeaconMap::internal_clear
virtual void internal_clear() MRPT_OVERRIDE
Internal method called by clear()
Definition: CBeaconMap.cpp:92
mrpt::maps::CBeaconMap::size
size_t size() const
Returns the stored landmarks count.
Definition: CBeaconMap.cpp:100
mrpt::maps::CBeaconMap::saveToMATLABScript3D
bool saveToMATLABScript3D(const std::string &file, const char *style="b", float confInterval=0.95f) const
Save to a MATLAB script which displays 3D error ellipses for the map.
Definition: CBeaconMap.cpp:849
mrpt::maps::CBeaconMap::changeCoordinatesReference
void changeCoordinatesReference(const mrpt::poses::CPose3D &newOrg)
Changes the reference system of the map to a given 3D pose.
Definition: CBeaconMap.cpp:754
mrpt::maps::CBeaconMap::internal_computeObservationLikelihood
double internal_computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom) MRPT_OVERRIDE
Internal method called by computeObservationLikelihood()
Definition: CBeaconMap.cpp:172
mrpt::maps::CBeaconMap::readFromStream
void readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
Definition: CBeaconMap.cpp:141
mrpt::maps::CBeaconMap::isEmpty
bool isEmpty() const MRPT_OVERRIDE
Returns true if the map is empty/no observation has been inserted.
Definition: CBeaconMap.cpp:984
mrpt::maps::CBeaconMap::iterator
std::deque< CBeacon >::iterator iterator
Definition: maps/CBeaconMap.h:51
mrpt::maps::CBeaconMap::computeMatchingWith3DLandmarks
void computeMatchingWith3DLandmarks(const mrpt::maps::CBeaconMap *otherMap, mrpt::utils::TMatchingPairList &correspondences, float &correspondencesRatio, std::vector< bool > &otherCorrespondences) const
Perform a search for correspondences between "this" and another lansmarks map: Firsly,...
Definition: CBeaconMap.cpp:775
mrpt::maps::CBeaconMap::internal_insertObservation
virtual bool internal_insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=NULL) MRPT_OVERRIDE
Internal method called by insertObservation()
Definition: CBeaconMap.cpp:346
mrpt::maps::CBeaconMap::m_beacons
TSequenceBeacons m_beacons
The individual beacons.
Definition: maps/CBeaconMap.h:55
mrpt::maps::CMetricMap
Declares a virtual base class for all metric maps storage classes.
Definition: maps/CMetricMap.h:55
mrpt::maps::CMetricMap::genericMapParams
TMapGenericParams genericMapParams
Common params to all maps.
Definition: maps/CMetricMap.h:227
mrpt::maps::CMetricMap::GetRuntimeClass
virtual const mrpt::utils::TRuntimeClassId * GetRuntimeClass() const
Returns information about the class of an object in runtime.
mrpt::maps::TMapGenericParams::enableSaveAs3DObject
bool enableSaveAs3DObject
(Default=true) If false, calling CMetricMap::getAs3DObject() will have no effects
Definition: metric_map_types.h:70
mrpt::math::CMatrixFixedNumeric
A numeric matrix of compile-time fixed size.
Definition: CMatrixFixedNumeric.h:43
mrpt::math::dynamic_vector
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
Definition: types_math.h:65
mrpt::obs::CObservationBeaconRanges
Declares a class derived from "CObservation" that represents one (or more) range measurements to labe...
Definition: obs/CObservationBeaconRanges.h:29
mrpt::obs::CObservationBeaconRanges::stdError
float stdError
The "sigma" of the sensor, assuming a zero-mean Gaussian noise model.
Definition: obs/CObservationBeaconRanges.h:37
mrpt::obs::CObservationBeaconRanges::sensedData
std::deque< TMeasurement > sensedData
The list of observed ranges.
Definition: obs/CObservationBeaconRanges.h:50
mrpt::obs::CObservation
Declares a class that represents any robot's observation.
Definition: obs/CObservation.h:41
mrpt::obs::CObservation::GetRuntimeClass
virtual const mrpt::utils::TRuntimeClassId * GetRuntimeClass() const
Returns information about the class of an object in runtime.
mrpt::opengl::CGridPlaneXY::Create
static CGridPlaneXYPtr Create()
mrpt::opengl::COpenGLScene
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives.
Definition: COpenGLScene.h:50
mrpt::opengl::COpenGLScene::insert
void insert(const CRenderizablePtr &newObject, const std::string &viewportName=std::string("main"))
Insert a new object into the scene, in the given viewport (by default, into the "main" viewport).
Definition: COpenGLScene.cpp:195
mrpt::opengl::CSetOfObjects::Create
static CSetOfObjectsPtr Create()
mrpt::poses::CPoint3D
A class used to store a 3D point.
Definition: CPoint3D.h:33
mrpt::poses::CPointPDFGaussian
A gaussian distribution for 3D points.
Definition: CPointPDFGaussian.h:27
mrpt::poses::CPointPDFGaussian::cov
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
Definition: CPointPDFGaussian.h:45
mrpt::poses::CPointPDFGaussian::mean
CPoint3D mean
The mean value.
Definition: CPointPDFGaussian.h:44
mrpt::poses::CPointPDFParticles::getCovarianceAndMean
void getCovarianceAndMean(mrpt::math::CMatrixDouble33 &cov, CPoint3D &mean_point) const MRPT_OVERRIDE
Returns an estimate of the point covariance matrix (3x3 cov matrix) and the mean, both at once.
Definition: CPointPDFParticles.cpp:90
mrpt::poses::CPointPDFParticles::setSize
void setSize(size_t numberParticles, const CPoint3D &defaultValue=CPoint3D(0, 0, 0))
Erase all the previous particles and change the number of particles, with a given initial value
Definition: CPointPDFParticles.cpp:41
mrpt::poses::CPointPDFParticles::clear
void clear()
Clear all the particles (free memory)
Definition: CPointPDFParticles.cpp:34
mrpt::poses::CPointPDFSOG::erase
iterator erase(iterator i)
Definition: CPointPDFSOG.h:108
mrpt::poses::CPointPDFSOG::size
size_t size() const
Return the number of Gaussian modes.
Definition: CPointPDFSOG.h:111
mrpt::poses::CPointPDFSOG::getCovarianceAndMean
void getCovarianceAndMean(mrpt::math::CMatrixDouble33 &cov, CPoint3D &mean_point) const MRPT_OVERRIDE
Returns an estimate of the point covariance matrix (3x3 cov matrix) and the mean, both at once.
Definition: CPointPDFSOG.cpp:96
mrpt::poses::CPointPDFSOG::const_iterator
std::deque< TGaussianMode >::const_iterator const_iterator
Definition: CPointPDFSOG.h:57
mrpt::poses::CPointPDFSOG::iterator
std::deque< TGaussianMode >::iterator iterator
Definition: CPointPDFSOG.h:58
mrpt::poses::CPointPDFSOG::normalizeWeights
void normalizeWeights()
Normalize the weights in m_modes such as the maximum log-weight is 0.
Definition: CPointPDFSOG.cpp:430
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:37
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:73
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
mrpt::poses::CPoseOrPoint::y_incr
void y_incr(const double v)
Definition: CPoseOrPoint.h:123
mrpt::poses::CPoseOrPoint::x_incr
void x_incr(const double v)
Definition: CPoseOrPoint.h:122
mrpt::poses::CPoseOrPoint::distance3DTo
double distance3DTo(double ax, double ay, double az) const
Returns the 3D distance from this pose/point to a 3D point.
Definition: CPoseOrPoint.h:168
mrpt::poses::CPoseOrPoint::distanceTo
double distanceTo(const CPoseOrPoint< OTHERCLASS > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:150
mrpt::random::CRandomGenerator::drawGaussian1D
double drawGaussian1D(const double mean, const double std)
Generate a normally distributed pseudo-random number.
Definition: RandomGenerators.h:139
mrpt::random::CRandomGenerator::drawUniform
double drawUniform(const double Min, const double Max)
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm,...
Definition: RandomGenerators.h:92
mrpt::utils::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: CConfigFileBase.h:34
mrpt::utils::CConfigFileBase::read_float
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:73
mrpt::utils::CFileOutputStream
This CStream derived class allow using a file as a write-only, binary stream.
Definition: CFileOutputStream.h:26
mrpt::utils::CStream
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:39
mrpt::utils::CStream::printf
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
mrpt::utils::CStringList
A class for storing a list of text lines.
Definition: CStringList.h:33
mrpt::utils::CStringList::getText
void getText(std::string &outText) const
Returns the whole string list as a single string with '\r ' characters for newlines.
Definition: CStringList.cpp:135
mrpt::utils::TMatchingPairList
A list of TMatchingPair.
Definition: TMatchingPair.h:79
data_utils.h
iterator
Scalar * iterator
Definition: eigen_plugins.h:23
const_iterator
const Scalar * const_iterator
Definition: eigen_plugins.h:24
begin
EIGEN_STRONG_INLINE iterator begin()
Definition: eigen_plugins.h:26
n
GLenum GLsizei n
Definition: glext.h:4618
obj
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
end
GLuint GLuint end
Definition: glext.h:3512
y
GLenum GLint GLint y
Definition: glext.h:3516
range
GLsizei range
Definition: glext.h:5281
in
GLuint in
Definition: glext.h:6301
p
GLfloat GLfloat p
Definition: glext.h:5587
s
GLdouble s
Definition: glext.h:3602
params
GLenum const GLfloat * params
Definition: glext.h:3514
string
GLsizei const GLchar ** string
Definition: glext.h:3919
source
GLsizei GLsizei GLchar * source
Definition: glext.h:3908
mrpt::math::isNaN
bool BASE_IMPEXP isNaN(float f) MRPT_NO_THROWS
Returns true if the number is NaN.
Definition: math.cpp:1679
mrpt::utils::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:26
mrpt::system::os::fprintf
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
mrpt::system::os::fopen
FILE BASE_IMPEXP * fopen(const char *fileName, const char *mode) MRPT_NO_THROWS
An OS-independent version of fopen.
Definition: os.cpp:255
mrpt::system::os::fclose
int BASE_IMPEXP void BASE_IMPEXP fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:272
mrpt::math::averageLogLikelihood
double BASE_IMPEXP averageLogLikelihood(const CVectorDouble &logLikelihoods)
A numerically-stable method to compute average likelihood values with strongly different ranges (unwe...
Definition: math.cpp:1840
version
int version
Definition: mrpt_jpeglib.h:898
MRPT_START
#define MRPT_START
Definition: mrpt_macros.h:366
ASSERT_
#define ASSERT_(f)
Definition: mrpt_macros.h:278
MRPT_END
#define MRPT_END
Definition: mrpt_macros.h:370
MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: mrpt_macros.h:217
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: mrpt_macros.h:154
MRPT_CHECK_NORMAL_NUMBER
#define MRPT_CHECK_NORMAL_NUMBER(val)
Definition: mrpt_macros.h:279
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
Definition: mrpt_macros.h:307
mrpt::bayes
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
Definition: CParticleFilter.h:23
mrpt::math
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:20
mrpt::math::CMatrixDouble
CMatrixTemplateNumeric< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
Definition: CMatrixTemplateNumeric.h:130
mrpt::math::square
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:18
mrpt::opengl::stock_objects::CornerXYZ
CSetOfObjectsPtr OPENGL_IMPEXP CornerXYZ(float scale=1.0)
Returns three arrows representing a X,Y,Z 3D corner.
Definition: StockObjects.cpp:129
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:18
mrpt::random
A namespace of pseudo-random numbers genrators of diferent distributions.
Definition: RandomGenerators.h:23
mrpt::random::randomGenerator
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:23
mrpt::system
This namespace provides a OS-independent interface to many useful functions: filenames manipulation,...
Definition: math_frwds.h:30
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values,...
Definition: zip.h:16
mrpt::utils::max3
const T max3(const T &A, const T &B, const T &C)
Definition: bits.h:129
mrpt::utils::clear
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:113
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:17
uint32_t
unsigned __int32 uint32_t
Definition: rptypes.h:49
stock_objects.h
mrpt::bayes::CParticleFilterDataImpl::performSubstitution
void performSubstitution(const std::vector< size_t > &indx) MRPT_OVERRIDE
Replaces the old particles by copies determined by the indexes in "indx", performing an efficient cop...
Definition: CParticleFilterData.h:98
mrpt::bayes::CParticleFilterDataImpl::ESS
double ESS() const MRPT_OVERRIDE
Returns the normalized ESS (Estimated Sample Size), in the range [0,1].
Definition: CParticleFilterData.h:78
mrpt::maps::CBeaconMap::TInsertionOptions::dumpToTextStream
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form,...
Definition: CBeaconMap.cpp:940
mrpt::maps::CBeaconMap::TInsertionOptions::TInsertionOptions
TInsertionOptions()
Initilization of default parameters.
Definition: CBeaconMap.cpp:922
mrpt::maps::CBeaconMap::TInsertionOptions::loadFromConfigFile
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: CBeaconMap.cpp:963
mrpt::maps::CBeaconMap::TLikelihoodOptions::loadFromConfigFile
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: CBeaconMap.cpp:912
mrpt::maps::CBeaconMap::TLikelihoodOptions::TLikelihoodOptions
TLikelihoodOptions()
Initilization of default parameters.
Definition: CBeaconMap.cpp:892
mrpt::maps::CBeaconMap::TLikelihoodOptions::dumpToTextStream
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form,...
Definition: CBeaconMap.cpp:900
mrpt::maps::CBeaconMap::TMapDefinition::loadFromConfigFile_map_specific
void loadFromConfigFile_map_specific(const mrpt::utils::CConfigFileBase &source, const std::string &sectionNamePrefix)
Load all map-specific params.
Definition: CBeaconMap.cpp:48
mrpt::maps::CBeaconMap::TMapDefinition::likelihoodOpts
mrpt::maps::CBeaconMap::TLikelihoodOptions likelihoodOpts
Probabilistic observation likelihood options.
Definition: maps/CBeaconMap.h:258
mrpt::maps::CBeaconMap::TMapDefinition::dumpToTextStream_map_specific
void dumpToTextStream_map_specific(mrpt::utils::CStream &out) const
Definition: CBeaconMap.cpp:58
mrpt::maps::CBeaconMap::TMapDefinition::insertionOpts
mrpt::maps::CBeaconMap::TInsertionOptions insertionOpts
Observations insertion options.
Definition: maps/CBeaconMap.h:257
mrpt::maps::TMatchingExtraResults
Additional results from the determination of matchings between point clouds, etc.,...
Definition: metric_map_types.h:46
mrpt::maps::TMatchingExtraResults::correspondencesRatio
float correspondencesRatio
The ratio [0,1] of points in otherMap with at least one correspondence.
Definition: metric_map_types.h:47
mrpt::maps::TMatchingParams
Parameters for the determination of matchings between point clouds, etc.
Definition: metric_map_types.h:23
mrpt::maps::TMatchingRatioParams
Parameters for CMetricMap::compute3DMatchingRatio()
Definition: metric_map_types.h:56
mrpt::maps::TMetricMapInitializer
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
Definition: TMetricMapInitializer.h:33
mrpt::obs::CObservationBeaconRanges::TMeasurement::sensorLocationOnRobot
mrpt::poses::CPoint3D sensorLocationOnRobot
Position of the sensor on the robot.
Definition: obs/CObservationBeaconRanges.h:45
mrpt::obs::CObservationBeaconRanges::TMeasurement::beaconID
int32_t beaconID
The ID of the sensed beacon (or INVALID_BEACON_ID if unknown)
Definition: obs/CObservationBeaconRanges.h:47
mrpt::obs::CObservationBeaconRanges::TMeasurement::sensedDistance
float sensedDistance
The sensed range itself (in meters).
Definition: obs/CObservationBeaconRanges.h:46
mrpt::utils::TMatchingPair
A structure for holding correspondences between two sets of points or points-like entities in 2D or 3...
Definition: TMatchingPair.h:32


Page generated by Doxygen 1.9.1 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at mar 26 may 2026 13:12:03 CEST