MRPT  2.0.4
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 }
mrpt::config
Definition: config/CConfigFile.h:14
mrpt::math::cov
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
os.h
filesystem.h
mrpt::obs::CRawlog
This class stores a rawlog (robotic datasets) in one of two possible formats:
Definition: CRawlog.h:65
COccupancyGridMap2D.h
mrpt::poses::CPosePDFParticles::resetUniform
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,...
Definition: CPosePDFParticles.cpp:209
mrpt::bayes::CParticleFilter::executeOn
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.
Definition: CParticleFilter.cpp:45
run_test_pf_localization
void run_test_pf_localization(CPose2D &meanPose, CMatrixDouble33 &cov)
Definition: CMonteCarloLocalization2D_unittest.cpp:37
mrpt::containers::end
const_iterator end() const
Definition: ts_hash_map.h:246
mrpt::io
Definition: img/CImage.h:24
mrpt::maps::CMultiMetricMap::mapByClass
T::Ptr mapByClass(size_t ith=0) const
Returns the i'th map of a given class (or of a derived class), or empty smart pointer if there is no ...
Definition: CMultiMetricMap.h:156
mrpt::math::MatrixBase::trace
Scalar trace() const
Returns the trace of the matrix (not necessarily square).
Definition: MatrixBase_impl.h:219
mrpt::system::CTicTac
A high-performance stopwatch, with typical resolution of nanoseconds.
Definition: system/CTicTac.h:17
mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations
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
mrpt::obs::CSensoryFrame::Ptr
std::shared_ptr< mrpt::obs ::CSensoryFrame > Ptr
Definition: CSensoryFrame.h:53
CMultiMetricMap.h
mrpt::bayes::CParticleFilter::TParticleFilterStats
Statistics for being returned from the "execute" method.
Definition: CParticleFilter.h:169
mrpt::bayes::CParticleFilter::m_options
CParticleFilter::TParticleFilterOptions m_options
The options to be used in the PF, must be set before executing any step of the particle filter.
Definition: CParticleFilter.h:209
CMonteCarloLocalization2D.h
RAWLOG_FILE
std::string RAWLOG_FILE
Definition: vision_stereo_rectify/test.cpp:45
mrpt::bayes::CParticleFilter
This class acts as a common interface to the different interfaces (see CParticleFilter::TParticleFilt...
Definition: CParticleFilter.h:51
CConfigFile.h
mrpt::slam::CMonteCarloLocalization2D
Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x,...
Definition: CMonteCarloLocalization2D.h:37
mrpt::obs::CActionCollection::Ptr
std::shared_ptr< mrpt::obs ::CActionCollection > Ptr
Definition: CActionCollection.h:28
THROW_EXCEPTION_FMT
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
ini_fil
const std::string ini_fil
Definition: RawlogGrabberApp_unittest.cpp:26
mrpt::system::fileExists
bool fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:128
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
mrpt::system::lowerCase
std::string lowerCase(const std::string &str)
Returns an lower-case version of a string.
Definition: string_utils.cpp:26
random.h
mrpt::random::CRandomGenerator::randomize
void randomize(const uint32_t seed)
Initialize the PRNG from the given random seed.
Definition: RandomGenerator.cpp:92
mrpt::maps::TSetOfMetricMapInitializers
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
Definition: TMetricMapInitializer.h:89
mrpt::slam::CMonteCarloLocalization2D::resetUniformFreeSpace
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...
Definition: CMonteCarloLocalization2D.cpp:223
mrpt::math::norm
CONTAINER::Scalar norm(const CONTAINER &v)
Definition: ops_containers.h:133
mrpt::obs::CRawlog::loadFromRawLogFile
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
mrpt::math::CMatrixFixed< double, 3, 3 >
mrpt::io::CFileGZInputStream
Transparently opens a compressed "gz" file and reads uncompressed data from it.
Definition: io/CFileGZInputStream.h:26
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:39
mrpt::maps::CMultiMetricMap
This class stores any customizable set of metric maps.
Definition: CMultiMetricMap.h:120
mrpt::obs::CRawlog::size
size_t size() const
Returns the number of actions / observations object in the sequence.
Definition: CRawlog.cpp:64
mrpt::bayes::CParticleFilter::TParticleFilterOptions::loadFromConfigFile
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: CParticleFilter.cpp:141
mrpt::obs::CRawlog::getActionObservationPair
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
iniFile
string iniFile(myDataDir+string("benchmark-options.ini"))
mrpt::maps::TSetOfMetricMapInitializers::loadFromConfigFile
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.
Definition: TMetricMapInitializer.cpp:69
mrpt::maps::CSimpleMap::size
size_t size() const
Returns the count of pairs (pose,sensory data)
Definition: CSimpleMap.cpp:53
mrpt::system::CTicTac::Tic
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:76
mrpt::poses::CPosePDFParticles::getCovarianceAndMean
std::tuple< cov_mat_t, type_value > getCovarianceAndMean() const override
Definition: CPosePDFParticles.cpp:108
mrpt::DEG2RAD
constexpr double DEG2RAD(const double x)
Degrees to radians
Definition: core/include/mrpt/core/bits_math.h:47
mrpt::system::extractFileExtension
std::string extractFileExtension(const std::string &filePath, bool ignore_gz=false)
Extract the extension of a filename.
Definition: filesystem.cpp:98
mrpt::maps::CMultiMetricMap::setListOfMaps
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 ...
Definition: CMultiMetricMap.cpp:113
TEST
TEST(MonteCarlo2D, RunSampleDataset)
Definition: CMonteCarloLocalization2D_unittest.cpp:264
mrpt::serialization::archiveFrom
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT's CStream,...
Definition: CArchive.h:592
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:89
mrpt::slam::CMonteCarloLocalization2D::options
TMonteCarloLocalizationParams options
MCL parameters.
Definition: CMonteCarloLocalization2D.h:45
mrpt::bayes
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
Definition: CKalmanFilterCapable.h:30
mrpt::slam::TMonteCarloLocalizationParams
The struct for passing extra simulation parameters to the prediction stage when running a particle fi...
Definition: TMonteCarloLocalizationParams.h:20
mrpt::config::CConfigFile
This class allows loading and storing values and vectors of different types from "....
Definition: config/CConfigFile.h:31
mrpt::maps::CSimpleMap
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:32
CFileGZInputStream.h
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::maps::COccupancyGridMap2D
A class for storing an occupancy grid map.
Definition: COccupancyGridMap2D.h:53
mrpt::random
A namespace of pseudo-random numbers generators of diferent distributions.
Definition: random_shuffle.h:18
mrpt::slam::TMonteCarloLocalizationParams::metricMap
mrpt::maps::CMetricMap * metricMap
[update stage] Must be set to a metric map used to estimate the likelihood of observations
Definition: TMonteCarloLocalizationParams.h:38
mrpt::maps
Definition: CBeacon.h:21
CArchive.h
mrpt::exception_to_str
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
CSimpleMap.h
mrpt::slam::TKLDParams::loadFromConfigFile
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
CRawlog.h
mrpt::slam
Definition: CMultiMetricMapPDF.h:26
mrpt::bayes::CParticleFilter::TParticleFilterOptions
The configuration of a particle filter.
Definition: CParticleFilter.h:102
mrpt::slam::TMonteCarloLocalizationParams::KLD_params
TKLDParams KLD_params
Parameters for dynamic sample size, KLD method.
Definition: TMonteCarloLocalizationParams.h:47
mrpt::system
Definition: backtrace.h:14



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sat Jun 27 14:00:59 UTC 2020