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-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include <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");
44  if (!mrpt::system::fileExists(ini_fil))
45  {
46  cerr << "WARNING: Skipping test due to missing file: " << ini_fil
47  << "\n";
48  return;
49  }
50 
51  CConfigFile iniFile(ini_fil);
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  pdf.getCovarianceAndMean(cov, meanPose);
251  // cout << meanPose << " cov trace: " << cov.trace() <<
252  // endl;
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 #if MRPT_IS_BIG_ENDIAN
267  MRPT_TODO("Debug this issue in big endian platforms")
268  return; // Skip this test for now
269 #endif
270 
271  // Actual ending point:
272  const CPose2D GT_endpose(15.904, -10.010, DEG2RAD(4.93));
273 
274  // Placeholder for results:
275  CPose2D meanPose;
277 
278  // Invoke test:
279  // Give it 3 opportunities, since it might fail once for bad luck, or even
280  // twice in an extreme bad luck:
281  for (int op = 0; op < 3; op++)
282  {
283  run_test_pf_localization(meanPose, cov);
284 
285  const double final_pf_cov_trace = cov.trace();
286  const CPose2D final_pf_pose = meanPose;
287 
288  bool pass1 = (final_pf_pose - GT_endpose).norm() < 0.10;
289  bool pass2 = final_pf_cov_trace < 0.01;
290 
291  if (pass1 && pass2) return; // OK!
292 
293  // else: give it another try...
294  cout << "\n*Warning: Test failed. Will give it another chance, since "
295  "after all it's nondeterministic!\n";
296  }
297 
298  FAIL() << "Failed to converge after 3 opportunities!!" << endl;
299 }
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 cube.
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:514
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:37
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:108
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:32
TEST(MonteCarlo2D, RunSampleDataset)
double DEG2RAD(const double x)
Degrees to radians.
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.
void getCovarianceAndMean(mrpt::math::CMatrixDouble33 &cov, CPose2D &mean_point) const override
Returns an estimate of the pose covariance matrix (3x3 cov matrix) and the mean, both at once...
TMonteCarloLocalizationParams options
MCL parameters.
STL namespace.
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:586
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:161
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:188
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...
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
GLuint GLuint end
Definition: glext.h:3532
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...
GLsizei const GLchar ** string
Definition: glext.h:4116
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.
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.
Eigen::Matrix< typename MATRIX::Scalar, MATRIX::ColsAtCompileTime, MATRIX::ColsAtCompileTime > 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:148
#define MRPT_TODO(x)
Definition: common.h:129
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:38
T::Ptr mapByClass(const 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 ...
The configuration of a particle filter.
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:110
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:86
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: bef67d9c5 Mon Apr 15 00:03:11 2019 +0200 at lun abr 15 00:10:13 CEST 2019