MRPT  2.0.4
test.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 
12 #include <mrpt/math/data_utils.h>
13 #include <mrpt/math/ops_vectors.h>
14 #include <mrpt/math/utils.h>
15 #include <mrpt/random.h>
16 #include <mrpt/system/filesystem.h>
17 #include <iostream>
18 #include <map>
19 
20 using namespace mrpt::bayes;
21 using namespace mrpt::math;
22 using namespace mrpt::random;
23 using namespace mrpt::system;
24 using namespace mrpt::io;
25 using namespace std;
26 
27 double MIN_LOG_WEIG = -1.0;
28 
29 unsigned int N_TESTS = 500;
30 int N_PARTICLES = 100;
31 
32 // For batch experiment:
34 map<string, CVectorDouble> results;
35 
36 // vectorToTextFile( out_indxs, #ALGOR, true, true); /* By rows, append */
37 
38 #define TEST_RESAMPLING(ALGOR) \
39  mrpt::system::deleteFile(#ALGOR); \
40  /*printf(#ALGOR);*/ \
41  /*printf("\n");*/ \
42  ERR_MEANs.clear(); \
43  ERR_STDs.clear(); \
44  for (size_t i = 0; i < N_TESTS; i++) \
45  { \
46  mrpt::random::getRandomGenerator().drawUniformVector( \
47  log_ws, MIN_LOG_WEIG, 0.0); \
48  CParticleFilterCapable::log2linearWeights(log_ws, lin_ws); \
49  CParticleFilterCapable::computeResampling( \
50  CParticleFilter::ALGOR, log_ws, out_indxs); \
51  hist_parts = mrpt::math::histogram(out_indxs, 0, M - 1, M, true); \
52  vector<double> errs_hist = lin_ws - hist_parts; \
53  ERR_MEANs.push_back(mrpt::math::mean(errs_hist)); \
54  ERR_STDs.push_back(mrpt::math::stddev(errs_hist)); \
55  } \
56  printf("%s: ERR_MEAN %e\n", #ALGOR, mrpt::math::mean(ERR_MEANs)); \
57  printf("%s: ERR_STD %f\n", #ALGOR, mrpt::math::mean(ERR_STDs)); \
58  results[#ALGOR].push_back(mrpt::math::mean(ERR_STDs));
59 
60 // ------------------------------------------------------
61 // TestResampling
62 // ------------------------------------------------------
63 void TestResampling()
64 {
65  vector<double> log_ws;
66  std::vector<size_t> out_indxs;
67 
68  const size_t M = N_PARTICLES;
69 
70  log_ws.resize(M);
71  // vectorToTextFile( log_ws, "log_ws.txt");
72 
73  // Compute normalized linear weights:
74  vector<double> lin_ws;
75  vector<double> hist_parts;
76  vector<double> ERR_MEANs;
77  vector<double> ERR_STDs;
78 
79  // prMultinomial
80  TEST_RESAMPLING(prMultinomial)
81  // prResidual
82  TEST_RESAMPLING(prResidual)
83  // prStratified
84  TEST_RESAMPLING(prStratified)
85  // prSystematic
86  TEST_RESAMPLING(prSystematic)
87 }
88 
89 void TestBatch()
90 {
91  for (double LL = -2; LL <= 2.01; LL += 0.08)
92  {
93  double L = pow(10.0, LL);
94 
96  printf("MIN_LOG_W=%f\n", L);
97 
98  MIN_LOG_WEIG = L;
100  }
101 
102  // Save results to files:
104 
105  vectorToTextFile(min_log_ws, "min_log_ws.txt");
106 
107  R = results["prMultinomial"];
108  vectorToTextFile(R, "prMultinomial.txt");
109  R = results["prResidual"];
110  vectorToTextFile(R, "prResidual.txt");
111  R = results["prStratified"];
112  vectorToTextFile(R, "prStratified.txt");
113  R = results["prSystematic"];
114  vectorToTextFile(R, "prSystematic.txt");
115 }
116 
117 // ------------------------------------------------------
118 // MAIN
119 // ------------------------------------------------------
120 int main(int argc, char** argv)
121 {
122  try
123  {
125 
126  if (argc > 1) N_PARTICLES = atoi(argv[1]);
127 
128  // TestResampling();
129  TestBatch();
130 
131  return 0;
132  }
133  catch (exception& e)
134  {
135  std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl;
136  return -1;
137  }
138  catch (...)
139  {
140  cerr << "Untyped excepcion!!";
141  return -1;
142  }
143 }
filesystem.h
mrpt::io
Definition: img/CImage.h:24
min_log_ws
CVectorDouble min_log_ws
Definition: vision_stereo_rectify/test.cpp:33
N_TESTS
unsigned int N_TESTS
Definition: vision_stereo_rectify/test.cpp:29
data_utils.h
MIN_LOG_WEIG
double MIN_LOG_WEIG
Definition: vision_stereo_rectify/test.cpp:27
R
const float R
Definition: CKinematicChain.cpp:137
vector_loadsave.h
results
map< string, CVectorDouble > results
Definition: vision_stereo_rectify/test.cpp:34
N_PARTICLES
int N_PARTICLES
Definition: vision_stereo_rectify/test.cpp:30
random.h
mrpt::random::CRandomGenerator::randomize
void randomize(const uint32_t seed)
Initialize the PRNG from the given random seed.
Definition: RandomGenerator.cpp:92
TestResampling
void TestResampling()
Definition: vision_stereo_rectify/test.cpp:63
main
int main()
Definition: vision_stereo_rectify/test.cpp:78
ops_vectors.h
TestBatch
void TestBatch()
Definition: vision_stereo_rectify/test.cpp:89
argv
const char * argv[]
Definition: RawlogGrabberApp_unittest.cpp:50
CParticleFilterCapable.h
utils.h
mrpt::math::CVectorDynamic< double >
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:89
argc
const int argc
Definition: RawlogGrabberApp_unittest.cpp:51
mrpt::bayes
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
Definition: CKalmanFilterCapable.h:31
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:12
mrpt::math::CVectorDynamic::push_back
void push_back(const T &val)
Definition: CVectorDynamic.h:197
mrpt::random
A namespace of pseudo-random numbers generators of diferent distributions.
Definition: random_shuffle.h:19
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
TEST_RESAMPLING
#define TEST_RESAMPLING(ALGOR)
Definition: vision_stereo_rectify/test.cpp:38
mrpt::system
Definition: backtrace.h:15
mrpt::io::vectorToTextFile
bool vectorToTextFile(const std::vector< float > &vec, const std::string &fileName, bool append=false, bool byRows=false)
A useful function for debugging, which saves a numeric std::vector as a plain-text file compatible wi...
Definition: vector_loadsave.cpp:91



Page generated by Doxygen 1.8.18 for MRPT 2.0.4 at Thu Sep 24 07:14:18 UTC 2020