Main MRPT website > C++ reference for MRPT 1.9.9
CMonteCarloLocalization2D_unittest.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-2018, 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 
14 #include <mrpt/maps/CSimpleMap.h>
15 #include <mrpt/obs/CRawlog.h>
16 #include <mrpt/system/filesystem.h>
17 #include <mrpt/system/os.h>
18 #include <mrpt/random.h>
19 #include <gtest/gtest.h>
20 
21 using namespace mrpt;
22 using namespace mrpt::bayes;
23 using namespace mrpt::slam;
24 using namespace mrpt::maps;
25 using namespace mrpt::io;
26 using namespace mrpt::poses;
27 using namespace mrpt::config;
28 using namespace mrpt::math;
29 using namespace mrpt::random;
30 using namespace mrpt::system;
31 using namespace mrpt::obs;
32 using namespace std;
33 
34 // Defined in tests/test_main.cpp
35 namespace mrpt
36 {
38 }
39 
41 {
42  // ------------------------------------------------------
43  // The code below is a simplification of the program "pf-localization"
44  // ------------------------------------------------------
45  const string ini_fil =
46  MRPT_GLOBAL_UNITTEST_SRC_DIR + string("/tests/montecarlo_test1.ini");
47  if (!mrpt::system::fileExists(ini_fil))
48  {
49  cerr << "WARNING: Skipping test due to missing file: " << ini_fil
50  << "\n";
51  return;
52  }
53 
54  CConfigFile iniFile(ini_fil);
55  std::vector<int>
56  particles_count; // Number of initial particles (if size>1, run
57  // the experiments N times)
58 
59  // Load configuration:
60  // -----------------------------------------
61  string iniSectionName("LocalizationExperiment");
62 
63  // Mandatory entries:
64  iniFile.read_vector(
65  iniSectionName, "particles_count", std::vector<int>(1, 0),
66  particles_count,
67  /*Fail if not found*/ true);
68  string RAWLOG_FILE = iniFile.read_string(
69  iniSectionName, "rawlog_file", "", /*Fail if not found*/ true);
70 
72 
73  // Non-mandatory entries:
74  string MAP_FILE = iniFile.read_string(iniSectionName, "map_file", "");
75 
76  MAP_FILE = MRPT_GLOBAL_UNITTEST_SRC_DIR + string("/") + MAP_FILE;
77 
78  size_t rawlog_offset = iniFile.read_int(iniSectionName, "rawlog_offset", 0);
79  int NUM_REPS = iniFile.read_int(iniSectionName, "experimentRepetitions", 1);
80 
81  // PF-algorithm Options:
82  // ---------------------------
84  pfOptions.loadFromConfigFile(iniFile, "PF_options");
85 
86  // PDF Options:
87  // ------------------
88  TMonteCarloLocalizationParams pdfPredictionOptions;
89  pdfPredictionOptions.KLD_params.loadFromConfigFile(iniFile, "KLD_options");
90 
91  // Metric map options:
92  // -----------------------------
94  mapList.loadFromConfigFile(iniFile, "MetricMap");
95 
96  // --------------------------------------------------------------------
97  // EXPERIMENT PREPARATION
98  // --------------------------------------------------------------------
99  CTicTac tictac, tictacGlobal;
100  CSimpleMap simpleMap;
101  CRawlog rawlog;
102  size_t rawlogEntry, rawlogEntries;
104 
105  // Load the set of metric maps to consider in the experiments:
106  CMultiMetricMap metricMap;
107  metricMap.setListOfMaps(&mapList);
108 
110 
111  // Load the map (if any):
112  // -------------------------
113  if (MAP_FILE.size())
114  {
115  ASSERT_(fileExists(MAP_FILE));
116 
117  // Detect file extension:
118  // -----------------------------
119  string mapExt = lowerCase(
121  MAP_FILE, true)); // Ignore possible .gz extensions
122 
123  if (!mapExt.compare("simplemap"))
124  {
125  // It's a ".simplemap":
126  // -------------------------
127  CFileGZInputStream f(MAP_FILE);
128  mrpt::serialization::archiveFrom(f) >> simpleMap;
129  ASSERT_(simpleMap.size() > 0);
130  // Build metric map:
131  metricMap.loadFromProbabilisticPosesAndObservations(simpleMap);
132  }
133  else if (!mapExt.compare("gridmap"))
134  {
135  // It's a ".gridmap":
136  // -------------------------
137  ASSERT_(metricMap.m_gridMaps.size() == 1);
138  CFileGZInputStream f(MAP_FILE);
139  mrpt::serialization::archiveFrom(f) >> (*metricMap.m_gridMaps[0]);
140  }
141  else
142  {
144  "Map file has unknown extension: '%s'", mapExt.c_str());
145  }
146  }
147 
148  // --------------------------
149  // Load the rawlog:
150  // --------------------------
152  rawlogEntries = rawlog.size();
153 
154  for (std::vector<int>::iterator itNum = particles_count.begin();
155  itNum != particles_count.end(); ++itNum)
156  {
157  int PARTICLE_COUNT = *itNum;
158 
159  // Global stats for all the experiment loops:
160  vector<double> covergenceErrors;
161  covergenceErrors.reserve(NUM_REPS);
162  // --------------------------------------------------------------------
163  // EXPERIMENT REPETITIONS LOOP
164  // --------------------------------------------------------------------
165  tictacGlobal.Tic();
166  for (int repetition = 0; repetition < NUM_REPS; repetition++)
167  {
168  int M = PARTICLE_COUNT;
170 
171  // PDF Options:
172  pdf.options = pdfPredictionOptions;
173 
174  pdf.options.metricMap = &metricMap;
175 
176  // Create the PF object:
177  CParticleFilter PF;
178  PF.m_options = pfOptions;
179 
180  size_t step = 0;
181  rawlogEntry = 0;
182 
183  // Initialize the PDF:
184  // -----------------------------
185  tictac.Tic();
186  if (!iniFile.read_bool(
187  iniSectionName, "init_PDF_mode", false,
188  /*Fail if not found*/ true))
190  metricMap.m_gridMaps[0].get(), 0.7f, PARTICLE_COUNT,
191  iniFile.read_float(
192  iniSectionName, "init_PDF_min_x", 0, true),
193  iniFile.read_float(
194  iniSectionName, "init_PDF_max_x", 0, true),
195  iniFile.read_float(
196  iniSectionName, "init_PDF_min_y", 0, true),
197  iniFile.read_float(
198  iniSectionName, "init_PDF_max_y", 0, true),
199  DEG2RAD(
200  iniFile.read_float(
201  iniSectionName, "init_PDF_min_phi_deg", -180)),
202  DEG2RAD(
203  iniFile.read_float(
204  iniSectionName, "init_PDF_max_phi_deg", 180)));
205  else
206  pdf.resetUniform(
207  iniFile.read_float(
208  iniSectionName, "init_PDF_min_x", 0, true),
209  iniFile.read_float(
210  iniSectionName, "init_PDF_max_x", 0, true),
211  iniFile.read_float(
212  iniSectionName, "init_PDF_min_y", 0, true),
213  iniFile.read_float(
214  iniSectionName, "init_PDF_max_y", 0, true),
215  DEG2RAD(
216  iniFile.read_float(
217  iniSectionName, "init_PDF_min_phi_deg", -180)),
218  DEG2RAD(
219  iniFile.read_float(
220  iniSectionName, "init_PDF_max_phi_deg", 180)),
221  PARTICLE_COUNT);
222 
223  // -----------------------------
224  // Particle filter
225  // -----------------------------
226  CActionCollection::Ptr action;
227  CSensoryFrame::Ptr observations;
228  bool end = false;
229 
230  // TTimeStamp cur_obs_timestamp;
231 
232  while (rawlogEntry < (rawlogEntries - 1) && !end)
233  {
234  // Load pose change from the rawlog:
235  // ----------------------------------------
236  if (!rawlog.getActionObservationPair(
237  action, observations, rawlogEntry))
238  THROW_EXCEPTION("End of rawlog");
239 
240  CPose2D expectedPose; // Ground truth
241 
242  // if (observations->size()>0)
243  // cur_obs_timestamp =
244  // observations->getObservationByIndex(0)->timestamp;
245 
246  if (step >= rawlog_offset)
247  {
248  // Do not execute the PF at "step=0", to let the initial PDF
249  // to be
250  // reflected in the logs.
251  if (step > rawlog_offset)
252  {
253  // ----------------------------------------
254  // RUN ONE STEP OF THE PARTICLE FILTER:
255  // ----------------------------------------
256  tictac.Tic();
257 
258  PF.executeOn(
259  pdf,
260  action.get(), // Action
261  observations.get(), // Obs.
262  &PF_stats // Output statistics
263  );
264  }
265 
266  pdf.getCovarianceAndMean(cov, meanPose);
267  // cout << meanPose << " cov trace: " << cov.trace() <<
268  // endl;
269 
270  } // end if rawlog_offset
271 
272  step++;
273 
274  }; // while rawlogEntries
275  } // for repetitions
276  } // end of loop for different # of particles
277 }
278 
279 // TEST =================
280 TEST(MonteCarlo2D, RunSampleDataset)
281 {
282 #if MRPT_IS_BIG_ENDIAN
283  MRPT_TODO("Debug this issue in big endian platforms")
284  return; // Skip this test for now
285 #endif
286 
287  // Actual ending point:
288  const CPose2D GT_endpose(15.904, -10.010, DEG2RAD(4.93));
289 
290  // Placeholder for results:
291  CPose2D meanPose;
293 
294  // Invoke test:
295  // Give it 3 opportunities, since it might fail once for bad luck, or even
296  // twice in an extreme bad luck:
297  for (int op = 0; op < 3; op++)
298  {
299  run_test_pf_localization(meanPose, cov);
300 
301  const double final_pf_cov_trace = cov.trace();
302  const CPose2D final_pf_pose = meanPose;
303 
304  bool pass1 = (final_pf_pose - GT_endpose).norm() < 0.10;
305  bool pass2 = final_pf_cov_trace < 0.01;
306 
307  if (pass1 && pass2) return; // OK!
308 
309  // else: give it another try...
310  cout << "\n*Warning: Test failed. Will give it another chance, since "
311  "after all it's nondeterministic!\n";
312  }
313 
314  FAIL() << "Failed to converge after 3 opportunities!!" << endl;
315 }
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.
Scalar * iterator
Definition: eigen_plugins.h:26
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:516
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
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:25
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:35
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:127
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:561
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
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:190
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:58
GLuint GLuint end
Definition: glext.h:3528
This class stores a rawlog (robotic datasets) in one of two possible formats:
Definition: CRawlog.h:68
This namespace contains representation of robot actions and observations.
string iniFile(myDataDir+string("benchmark-options.ini"))
ProxyFilterContainerByClass< mrpt::maps::COccupancyGridMap2D::Ptr, TListMaps > m_gridMaps
STL-like proxy to access this kind of maps in maps.
std::string extractFileExtension(const std::string &filePath, bool ignore_gz=false)
Extract the extension of a filename.
Definition: filesystem.cpp:97
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:4101
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...
std::string MRPT_GLOBAL_UNITTEST_SRC_DIR
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.
#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:40
void setListOfMaps(const mrpt::maps::TSetOfMetricMapInitializers *initializers)
Sets the list of internal map according to the passed list of map initializers (Current maps&#39; content...
The configuration of a particle filter.
Transparently opens a compressed "gz" file and reads uncompressed data from it.
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
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:79
This class stores any customizable set of metric maps.
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:43
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:88
TKLDParams KLD_params
Parameters for dynamic sample size, KLD method.
CONTAINER::Scalar norm(const CONTAINER &v)



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019