Main MRPT website > C++ reference for MRPT 1.5.6
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-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 
12 #include <mrpt/utils/CConfigFile.h>
15 #include <mrpt/maps/CSimpleMap.h>
16 #include <mrpt/obs/CRawlog.h>
17 #include <mrpt/system/filesystem.h>
18 #include <mrpt/system/os.h>
19 #include <mrpt/random.h>
20 #include <gtest/gtest.h>
21 
22 using namespace mrpt;
23 using namespace mrpt::bayes;
24 using namespace mrpt::slam;
25 using namespace mrpt::maps;
26 using namespace mrpt::utils;
27 using namespace mrpt::poses;
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 { namespace utils {
37  }
38 }
39 
40 
42 {
43 // ------------------------------------------------------
44 // The code below is a simplification of the program "pf-localization"
45 // ------------------------------------------------------
46  const string ini_fil = 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 << "\n";
50  return;
51  }
52 
53  CConfigFile iniFile(ini_fil);
54  vector_int particles_count; // Number of initial particles (if size>1, run the experiments N times)
55 
56  // Load configuration:
57  // -----------------------------------------
58  string iniSectionName ( "LocalizationExperiment" );
59 
60  // Mandatory entries:
61  iniFile.read_vector(iniSectionName, "particles_count", vector_int(1,0), particles_count, /*Fail if not found*/true );
62  string RAWLOG_FILE = iniFile.read_string(iniSectionName,"rawlog_file","", /*Fail if not found*/true );
63 
64  RAWLOG_FILE = MRPT_GLOBAL_UNITTEST_SRC_DIR + string("/") + RAWLOG_FILE;
65 
66  // Non-mandatory entries:
67  string MAP_FILE = iniFile.read_string(iniSectionName,"map_file","" );
68 
69  MAP_FILE = MRPT_GLOBAL_UNITTEST_SRC_DIR + string("/") + MAP_FILE;
70 
71  size_t rawlog_offset = iniFile.read_int(iniSectionName,"rawlog_offset",0);
72  int NUM_REPS = iniFile.read_int(iniSectionName,"experimentRepetitions",1);
73 
74  // PF-algorithm Options:
75  // ---------------------------
77  pfOptions.loadFromConfigFile( iniFile, "PF_options" );
78 
79  // PDF Options:
80  // ------------------
81  TMonteCarloLocalizationParams pdfPredictionOptions;
82  pdfPredictionOptions.KLD_params.loadFromConfigFile( iniFile, "KLD_options");
83 
84  // Metric map options:
85  // -----------------------------
87  mapList.loadFromConfigFile( iniFile,"MetricMap");
88 
89  // --------------------------------------------------------------------
90  // EXPERIMENT PREPARATION
91  // --------------------------------------------------------------------
92  CTicTac tictac,tictacGlobal;
93  CSimpleMap simpleMap;
94  CRawlog rawlog;
95  size_t rawlogEntry, rawlogEntries;
97 
98  // Load the set of metric maps to consider in the experiments:
99  CMultiMetricMap metricMap;
100  metricMap.setListOfMaps( &mapList );
101 
103 
104  // Load the map (if any):
105  // -------------------------
106  if (MAP_FILE.size())
107  {
108  ASSERT_( fileExists(MAP_FILE) );
109 
110  // Detect file extension:
111  // -----------------------------
112  string mapExt = lowerCase( extractFileExtension( MAP_FILE, true ) ); // Ignore possible .gz extensions
113 
114  if ( !mapExt.compare( "simplemap" ) )
115  {
116  // It's a ".simplemap":
117  // -------------------------
118  CFileGZInputStream(MAP_FILE.c_str()) >> simpleMap;
119 
120  ASSERT_( simpleMap.size()>0 );
121 
122  // Build metric map:
123  // ------------------------------
124  metricMap.loadFromProbabilisticPosesAndObservations(simpleMap);
125  }
126  else if ( !mapExt.compare( "gridmap" ) )
127  {
128  // It's a ".gridmap":
129  // -------------------------
130  ASSERT_( metricMap.m_gridMaps.size()==1 );
131  CFileGZInputStream(MAP_FILE) >> (*metricMap.m_gridMaps[0]);
132  }
133  else
134  {
135  THROW_EXCEPTION_FMT("Map file has unknown extension: '%s'",mapExt.c_str());
136  }
137 
138  }
139 
140  // --------------------------
141  // Load the rawlog:
142  // --------------------------
143  rawlog.loadFromRawLogFile(RAWLOG_FILE);
144  rawlogEntries = rawlog.size();
145 
146  for ( vector_int::iterator itNum = particles_count.begin(); itNum!=particles_count.end(); ++itNum )
147  {
148  int PARTICLE_COUNT = *itNum;
149 
150 
151  // Global stats for all the experiment loops:
152  vector<double> covergenceErrors;
153  covergenceErrors.reserve(NUM_REPS);
154  // --------------------------------------------------------------------
155  // EXPERIMENT REPETITIONS LOOP
156  // --------------------------------------------------------------------
157  tictacGlobal.Tic();
158  for (int repetition = 0; repetition <NUM_REPS; repetition++)
159  {
160  int M = PARTICLE_COUNT;
162 
163  // PDF Options:
164  pdf.options = pdfPredictionOptions;
165 
166  pdf.options.metricMap = &metricMap;
167 
168  // Create the PF object:
169  CParticleFilter PF;
170  PF.m_options = pfOptions;
171 
172  size_t step = 0;
173  rawlogEntry = 0;
174 
175  // Initialize the PDF:
176  // -----------------------------
177  tictac.Tic();
178  if ( !iniFile.read_bool(iniSectionName,"init_PDF_mode",false, /*Fail if not found*/true) )
180  metricMap.m_gridMaps[0].pointer(),
181  0.7f,
182  PARTICLE_COUNT ,
183  iniFile.read_float(iniSectionName,"init_PDF_min_x",0,true),
184  iniFile.read_float(iniSectionName,"init_PDF_max_x",0,true),
185  iniFile.read_float(iniSectionName,"init_PDF_min_y",0,true),
186  iniFile.read_float(iniSectionName,"init_PDF_max_y",0,true),
187  DEG2RAD(iniFile.read_float(iniSectionName,"init_PDF_min_phi_deg",-180)),
188  DEG2RAD(iniFile.read_float(iniSectionName,"init_PDF_max_phi_deg",180))
189  );
190  else
191  pdf.resetUniform(
192  iniFile.read_float(iniSectionName,"init_PDF_min_x",0,true),
193  iniFile.read_float(iniSectionName,"init_PDF_max_x",0,true),
194  iniFile.read_float(iniSectionName,"init_PDF_min_y",0,true),
195  iniFile.read_float(iniSectionName,"init_PDF_max_y",0,true),
196  DEG2RAD(iniFile.read_float(iniSectionName,"init_PDF_min_phi_deg",-180)),
197  DEG2RAD(iniFile.read_float(iniSectionName,"init_PDF_max_phi_deg",180)),
198  PARTICLE_COUNT
199  );
200 
201 
202  // -----------------------------
203  // Particle filter
204  // -----------------------------
205  CActionCollectionPtr action;
206  CSensoryFramePtr observations;
207  bool end = false;
208 
209  //TTimeStamp cur_obs_timestamp;
210 
211  while (rawlogEntry<(rawlogEntries-1) && !end)
212  {
213  // Finish if ESC is pushed:
214  if (os::kbhit())
215  if (os::getch()==27)
216  end = true;
217 
218  // Load pose change from the rawlog:
219  // ----------------------------------------
220  if (!rawlog.getActionObservationPair(action, observations, rawlogEntry ))
221  THROW_EXCEPTION("End of rawlog");
222 
223  CPose2D expectedPose; // Ground truth
224 
225 // if (observations->size()>0)
226 // cur_obs_timestamp = observations->getObservationByIndex(0)->timestamp;
227 
228  if (step>=rawlog_offset)
229  {
230  // Do not execute the PF at "step=0", to let the initial PDF to be
231  // reflected in the logs.
232  if (step>rawlog_offset)
233  {
234 
235  // ----------------------------------------
236  // RUN ONE STEP OF THE PARTICLE FILTER:
237  // ----------------------------------------
238  tictac.Tic();
239 
240  PF.executeOn(
241  pdf,
242  action.pointer(), // Action
243  observations.pointer(), // Obs.
244  &PF_stats // Output statistics
245  );
246 
247  }
248 
249  pdf.getCovarianceAndMean(cov,meanPose);
250  //cout << meanPose << " cov trace: " << cov.trace() << endl;
251 
252  } // end if rawlog_offset
253 
254  step++;
255 
256  }; // while rawlogEntries
257  } // for repetitions
258  } // end of loop for different # of particles
259 
260 }
261 
262 // TEST =================
263 TEST(MonteCarlo2D, RunSampleDataset)
264 {
265 #if MRPT_IS_BIG_ENDIAN
266  MRPT_TODO("Debug this issue in big endian platforms")
267  return; // Skip this test for now
268 #endif
269 
270  // Actual ending point:
271  const CPose2D GT_endpose(15.904,-10.010,DEG2RAD(4.93));
272 
273  // Placeholder for results:
274  CPose2D meanPose;
276 
277  // Invoke test:
278  // Give it 3 opportunities, since it might fail once for bad luck, or even twice in an extreme bad luck:
279  for (int op=0;op<3;op++)
280  {
281  run_test_pf_localization(meanPose,cov);
282 
283  const double final_pf_cov_trace = cov.trace();
284  const CPose2D final_pf_pose = meanPose;
285 
286  bool pass1 = (final_pf_pose-GT_endpose).norm() < 0.10;
287  bool pass2 = final_pf_cov_trace < 0.01;
288 
289  if (pass1 && pass2)
290  return; // OK!
291 
292  // else: give it another try...
293  cout << "\n*Warning: Test failed. Will give it another chance, since after all it's nondeterministic!\n";
294  }
295 
296  FAIL() << "Failed to converge after 3 opportunities!!" << endl;
297 }
298 
size_t size() const
Returns the number of actions / observations object in the sequence.
Definition: CRawlog.cpp:83
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
std::string MRPT_GLOBAL_UNITTEST_SRC_DIR
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...
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:48
void getCovarianceAndMean(mrpt::math::CMatrixDouble33 &cov, CPose2D &mean_point) const MRPT_OVERRIDE
Returns an estimate of the pose covariance matrix (3x3 cov matrix) and the mean, both at once...
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
bool getActionObservationPair(CActionCollectionPtr &action, CSensoryFramePtr &observations, size_t &rawlogEntry) const
Gets the next consecutive pair action / observation from the rawlog loaded into this object...
Definition: CRawlog.cpp:531
TEST(MonteCarlo2D, RunSampleDataset)
This class allows loading and storing values and vectors of different types from ".ini" files easily.
Definition: CConfigFile.h:30
#define THROW_EXCEPTION(msg)
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
void randomize(const uint32_t seed)
Initialize the PRNG from the given random seed.
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
Scalar * iterator
Definition: eigen_plugins.h:23
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.
bool BASE_IMPEXP fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:124
void read_vector(const std::string &section, const std::string &name, const VECTOR_TYPE &defaultValue, VECTOR_TYPE &outValues, bool failIfNotFound=false) const
Reads a configuration parameter of type vector, stored in the file as a string: "[v1 v2 v3 ...
TMonteCarloLocalizationParams options
MCL parameters.
Statistics for being returned from the "execute" method.
ProxyFilterContainerByClass< mrpt::maps::COccupancyGridMap2DPtr, TListMaps > m_gridMaps
STL-like proxy to access this kind of maps in maps.
void Tic()
Starts the stopwatch.
Definition: CTicTac.cpp:77
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)
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
MRPT_TODO("Modify ping to run on Windows + Test this")
size_t size() const
Returns the count of pairs (pose,sensory data)
Definition: CSimpleMap.cpp:67
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:204
int BASE_IMPEXP getch() MRPT_NO_THROWS
An OS-independent version of getch, which waits until a key is pushed.
Definition: os.cpp:375
void executeOn(CParticleFilterCapable &obj, const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, TParticleFilterStats *stats=NULL)
Executes a complete prediction + update step of the selected particle filtering algorithm.
GLuint GLuint end
Definition: glew.h:1167
std::string BASE_IMPEXP lowerCase(const std::string &str)
Returns an lower-case version of a string.
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &sectionName) MRPT_OVERRIDE
Loads the configuration for the set of internal maps from a textual definition in an INI-like file...
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:135
Transparently opens a compressed "gz" file and reads uncompressed data from it.
This class implements a high-performance stopwatch.
Definition: CTicTac.h:24
This class stores a rawlog (robotic datasets) in one of two possible formats:
Definition: obs/CRawlog.h:52
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.
std::string BASE_IMPEXP extractFileExtension(const std::string &filePath, bool ignore_gz=false)
Extract the extension of a filename.
Definition: filesystem.cpp:96
This class acts as a common interface to the different interfaces (see CParticleFilter::TParticleFilt...
#define DEG2RAD
Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x...
GLsizei const GLcharARB ** string
Definition: glew.h:3293
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
void setListOfMaps(const mrpt::maps::TSetOfMetricMapInitializers *initializers)
Sets the list of internal map according to the passed list of map initializers (Current maps' content...
bool BASE_IMPEXP kbhit() MRPT_NO_THROWS
An OS-independent version of kbhit, which returns true if a key has been pushed.
Definition: os.cpp:396
The configuration of a particle filter.
#define ASSERT_(f)
std::vector< int32_t > vector_int
Definition: types_simple.h:23
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
This class stores any customizable set of metric maps.
CParticleFilter::TParticleFilterOptions m_options
The options to be used in the PF, must be set before executing any step of the particle filter...
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: TKLDParams.cpp:53
TKLDParams KLD_params
Parameters for dynamic sample size, KLD method.
CONTAINER::Scalar norm(const CONTAINER &v)



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018