MRPT  1.9.9
CMonteCarloLocalization2D_unittest.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include <gtest/gtest.h>
15 #include <mrpt/maps/CSimpleMap.h>
16 #include <mrpt/obs/CRawlog.h>
17 #include <mrpt/random.h>
20 #include <mrpt/system/filesystem.h>
21 #include <mrpt/system/os.h>
22 #include <test_mrpt_common.h>
23 
24 using namespace mrpt;
25 using namespace mrpt::bayes;
26 using namespace mrpt::slam;
27 using namespace mrpt::maps;
28 using namespace mrpt::io;
29 using namespace mrpt::poses;
30 using namespace mrpt::config;
31 using namespace mrpt::math;
32 using namespace mrpt::random;
33 using namespace mrpt::system;
34 using namespace mrpt::obs;
35 using namespace std;
36 
38 {
39  // ------------------------------------------------------
40  // The code below is a simplification of the program "pf-localization"
41  // ------------------------------------------------------
42  const string ini_fil =
43  UNITTEST_BASEDIR + string("/tests/montecarlo_test1.ini");
45  {
46  cerr << "WARNING: Skipping test due to missing file: " << ini_fil
47  << "\n";
48  return;
49  }
50 
52  std::vector<int>
53  particles_count; // Number of initial particles (if size>1, run
54  // the experiments N times)
55 
56  // Load configuration:
57  // -----------------------------------------
58  string iniSectionName("LocalizationExperiment");
59 
60  // Mandatory entries:
61  iniFile.read_vector(
62  iniSectionName, "particles_count", std::vector<int>(1, 0),
63  particles_count,
64  /*Fail if not found*/ true);
65  string RAWLOG_FILE = iniFile.read_string(
66  iniSectionName, "rawlog_file", "", /*Fail if not found*/ true);
67 
68  RAWLOG_FILE = UNITTEST_BASEDIR + string("/") + RAWLOG_FILE;
69 
70  // Non-mandatory entries:
71  string MAP_FILE = iniFile.read_string(iniSectionName, "map_file", "");
72 
73  MAP_FILE = UNITTEST_BASEDIR + string("/") + MAP_FILE;
74 
75  size_t rawlog_offset = iniFile.read_int(iniSectionName, "rawlog_offset", 0);
76  int NUM_REPS = iniFile.read_int(iniSectionName, "experimentRepetitions", 1);
77 
78  // PF-algorithm Options:
79  // ---------------------------
81  pfOptions.loadFromConfigFile(iniFile, "PF_options");
82 
83  // PDF Options:
84  // ------------------
85  TMonteCarloLocalizationParams pdfPredictionOptions;
86  pdfPredictionOptions.KLD_params.loadFromConfigFile(iniFile, "KLD_options");
87 
88  // Metric map options:
89  // -----------------------------
91  mapList.loadFromConfigFile(iniFile, "MetricMap");
92 
93  // --------------------------------------------------------------------
94  // EXPERIMENT PREPARATION
95  // --------------------------------------------------------------------
96  CTicTac tictac, tictacGlobal;
97  CSimpleMap simpleMap;
98  CRawlog rawlog;
99  size_t rawlogEntry, rawlogEntries;
101 
102  // Load the set of metric maps to consider in the experiments:
103  CMultiMetricMap metricMap;
104  metricMap.setListOfMaps(mapList);
105 
107 
108  // Load the map (if any):
109  // -------------------------
110  if (MAP_FILE.size())
111  {
112  ASSERT_(fileExists(MAP_FILE));
113 
114  // Detect file extension:
115  // -----------------------------
116  string mapExt = lowerCase(extractFileExtension(
117  MAP_FILE, true)); // Ignore possible .gz extensions
118 
119  if (!mapExt.compare("simplemap"))
120  {
121  // It's a ".simplemap":
122  // -------------------------
123  CFileGZInputStream f(MAP_FILE);
124  mrpt::serialization::archiveFrom(f) >> simpleMap;
125  ASSERT_(simpleMap.size() > 0);
126  // Build metric map:
127  metricMap.loadFromProbabilisticPosesAndObservations(simpleMap);
128  }
129  else
130  {
132  "Map file has unknown extension: '%s'", mapExt.c_str());
133  }
134  }
135 
136  // --------------------------
137  // Load the rawlog:
138  // --------------------------
140  rawlogEntries = rawlog.size();
141 
142  for (int PARTICLE_COUNT : particles_count)
143  {
144  // Global stats for all the experiment loops:
145  vector<double> covergenceErrors;
146  covergenceErrors.reserve(NUM_REPS);
147  // --------------------------------------------------------------------
148  // EXPERIMENT REPETITIONS LOOP
149  // --------------------------------------------------------------------
150  tictacGlobal.Tic();
151  for (int repetition = 0; repetition < NUM_REPS; repetition++)
152  {
153  int M = PARTICLE_COUNT;
155 
156  // PDF Options:
157  pdf.options = pdfPredictionOptions;
158 
159  pdf.options.metricMap = &metricMap;
160 
161  // Create the PF object:
162  CParticleFilter PF;
163  PF.m_options = pfOptions;
164 
165  size_t step = 0;
166  rawlogEntry = 0;
167 
168  // Initialize the PDF:
169  // -----------------------------
170  tictac.Tic();
171  if (!iniFile.read_bool(
172  iniSectionName, "init_PDF_mode", false,
173  /*Fail if not found*/ true))
174  {
175  auto grid = metricMap.mapByClass<COccupancyGridMap2D>();
177  grid.get(), 0.7f, PARTICLE_COUNT,
178  iniFile.read_float(
179  iniSectionName, "init_PDF_min_x", 0, true),
180  iniFile.read_float(
181  iniSectionName, "init_PDF_max_x", 0, true),
182  iniFile.read_float(
183  iniSectionName, "init_PDF_min_y", 0, true),
184  iniFile.read_float(
185  iniSectionName, "init_PDF_max_y", 0, true),
186  DEG2RAD(iniFile.read_float(
187  iniSectionName, "init_PDF_min_phi_deg", -180)),
188  DEG2RAD(iniFile.read_float(
189  iniSectionName, "init_PDF_max_phi_deg", 180)));
190  }
191  else
192  pdf.resetUniform(
193  iniFile.read_float(
194  iniSectionName, "init_PDF_min_x", 0, true),
195  iniFile.read_float(
196  iniSectionName, "init_PDF_max_x", 0, true),
197  iniFile.read_float(
198  iniSectionName, "init_PDF_min_y", 0, true),
199  iniFile.read_float(
200  iniSectionName, "init_PDF_max_y", 0, true),
201  DEG2RAD(iniFile.read_float(
202  iniSectionName, "init_PDF_min_phi_deg", -180)),
203  DEG2RAD(iniFile.read_float(
204  iniSectionName, "init_PDF_max_phi_deg", 180)),
205  PARTICLE_COUNT);
206 
207  // -----------------------------
208  // Particle filter
209  // -----------------------------
210  CActionCollection::Ptr action;
211  CSensoryFrame::Ptr observations;
212  bool end = false;
213 
214  // TTimeStamp cur_obs_timestamp;
215 
216  while (rawlogEntry < (rawlogEntries - 1) && !end)
217  {
218  // Load pose change from the rawlog:
219  // ----------------------------------------
220  if (!rawlog.getActionObservationPair(
221  action, observations, rawlogEntry))
222  THROW_EXCEPTION("End of rawlog");
223 
224  CPose2D expectedPose; // Ground truth
225 
226  // if (observations->size()>0)
227  // cur_obs_timestamp =
228  // observations->getObservationByIndex(0)->timestamp;
229 
230  if (step >= rawlog_offset)
231  {
232  // Do not execute the PF at "step=0", to let the initial PDF
233  // to be
234  // reflected in the logs.
235  if (step > rawlog_offset)
236  {
237  // ----------------------------------------
238  // RUN ONE STEP OF THE PARTICLE FILTER:
239  // ----------------------------------------
240  tictac.Tic();
241 
242  PF.executeOn(
243  pdf,
244  action.get(), // Action
245  observations.get(), // Obs.
246  &PF_stats // Output statistics
247  );
248  }
249 
250  const auto [C, M] = pdf.getCovarianceAndMean();
251  cov = C;
252  meanPose = M;
253 
254  } // end if rawlog_offset
255 
256  step++;
257 
258  }; // while rawlogEntries
259  } // for repetitions
260  } // end of loop for different # of particles
261 }
262 
263 // TEST =================
264 TEST(MonteCarlo2D, RunSampleDataset)
265 {
266  try
267  {
268  // Actual ending point:
269  const CPose2D GT_endpose(15.904, -10.010, 4.93_deg);
270 
271  // Placeholder for results:
272  CPose2D meanPose;
274 
275  // Invoke test:
276  // Give it 3 opportunities, since it might fail once for bad luck, or
277  // even twice in an extreme bad luck:
278  for (int op = 0; op < 3; op++)
279  {
280  run_test_pf_localization(meanPose, cov);
281 
282  const double final_pf_cov_trace = cov.trace();
283  const CPose2D final_pf_pose = meanPose;
284 
285  bool pass1 = (final_pf_pose - GT_endpose).norm() < 0.10;
286  bool pass2 = final_pf_cov_trace < 0.01;
287 
288  if (pass1 && pass2) return; // OK!
289 
290  // else: give it another try...
291  cout << "\n*Warning: Test failed. Will give it another chance, "
292  "since "
293  "after all it's nondeterministic!\n";
294  }
295 
296  FAIL() << "Failed to converge after 3 opportunities!!" << endl;
297  }
298  catch (const std::exception& e)
299  {
300  FAIL() << mrpt::exception_to_str(e);
301  }
302 }
A namespace of pseudo-random numbers generators of diferent distributions.
void resetUniform(const double x_min, const double x_max, const double y_min, const double y_max, const double phi_min=-M_PI, const double phi_max=M_PI, const int particlesCount=-1)
Reset the PDF to an uniformly distributed one, inside of the defined 2D area [x_min,x_max]x[y_min,y_max] (in meters) and for orientations [phi_min, phi_max] (in radians).
std::tuple< cov_mat_t, type_value > getCovarianceAndMean() const override
Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean...
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
The struct for passing extra simulation parameters to the prediction stage when running a particle fi...
void resetUniformFreeSpace(mrpt::maps::COccupancyGridMap2D *theMap, const double freeCellsThreshold=0.7, const int particlesCount=-1, const double x_min=-1e10f, const double x_max=1e10f, const double y_min=-1e10f, const double y_max=1e10f, const double phi_min=-M_PI, const double phi_max=M_PI)
Reset the PDF to an uniformly distributed one, but only in the free-space of a given 2D occupancy-gri...
bool getActionObservationPair(CActionCollection::Ptr &action, CSensoryFrame::Ptr &observations, size_t &rawlogEntry) const
Gets the next consecutive pair action / observation from the rawlog loaded into this object...
Definition: CRawlog.cpp:492
std::string RAWLOG_FILE
void loadFromProbabilisticPosesAndObservations(const mrpt::maps::CSimpleMap &Map)
Load the map contents from a CSimpleMap object, erasing all previous content of the map...
Definition: CMetricMap.cpp:36
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:32
TEST(MonteCarlo2D, RunSampleDataset)
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
void randomize(const uint32_t seed)
Initialize the PRNG from the given random seed.
This class allows loading and storing values and vectors of different types from ".ini" files easily.
bool fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:128
A high-performance stopwatch, with typical resolution of nanoseconds.
TMonteCarloLocalizationParams options
MCL parameters.
STL namespace.
T::Ptr mapByClass(size_t ith=0) const
Returns the i&#39;th map of a given class (or of a derived class), or empty smart pointer if there is no ...
Statistics for being returned from the "execute" method.
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
mrpt::maps::CMetricMap * metricMap
[update stage] Must be set to a metric map used to estimate the likelihood of observations ...
void run_test_pf_localization(CPose2D &meanPose, CMatrixDouble33 &cov)
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT&#39;s CStream, std::istream, std::ostream, std::stringstream.
Definition: CArchive.h:592
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
bool loadFromRawLogFile(const std::string &fileName, bool non_obs_objects_are_legal=false)
Load the contents from a file containing one of these possibilities:
Definition: CRawlog.cpp:166
This base provides a set of functions for maths stuff.
std::string lowerCase(const std::string &str)
Returns an lower-case version of a string.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &sectionName) override
Loads the configuration for the set of internal maps from a textual definition in an INI-like file...
constexpr double DEG2RAD(const double x)
Degrees to radians.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
Definition: TKLDParams.cpp:48
const std::string ini_fil
This class stores a rawlog (robotic datasets) in one of two possible formats:
Definition: CRawlog.h:65
This namespace contains representation of robot actions and observations.
string iniFile(myDataDir+string("benchmark-options.ini"))
std::string extractFileExtension(const std::string &filePath, bool ignore_gz=false)
Extract the extension of a filename.
Definition: filesystem.cpp:98
size_t size() const
Returns the count of pairs (pose,sensory data)
Definition: CSimpleMap.cpp:53
This class acts as a common interface to the different interfaces (see CParticleFilter::TParticleFilt...
CMatrixDouble cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...
Definition: ops_matrices.h:149
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x...
A class for storing an occupancy grid map.
const_iterator end() const
Definition: ts_hash_map.h:246
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void executeOn(CParticleFilterCapable &obj, const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, TParticleFilterStats *stats=nullptr)
Executes a complete prediction + update step of the selected particle filtering algorithm.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
The configuration of a particle filter.
Scalar trace() const
Returns the trace of the matrix (not necessarily square).
std::string exception_to_str(const std::exception &e)
Builds a nice textual representation of a nested exception, which if generated using MRPT macros (THR...
Definition: exceptions.cpp:59
Transparently opens a compressed "gz" file and reads uncompressed data from it.
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:75
This class stores any customizable set of metric maps.
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
CParticleFilter::TParticleFilterOptions m_options
The options to be used in the PF, must be set before executing any step of the particle filter...
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
size_t size() const
Returns the number of actions / observations object in the sequence.
Definition: CRawlog.cpp:64
TKLDParams KLD_params
Parameters for dynamic sample size, KLD method.
void setListOfMaps(const mrpt::maps::TSetOfMetricMapInitializers &init)
Sets the list of internal map according to the passed list of map initializers (current maps will be ...
CONTAINER::Scalar norm(const CONTAINER &v)



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020