Main MRPT website > C++ reference for MRPT 1.5.6
RandomGenerator.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 #include "base-precomp.h" // Precompiled headers
11 
13 #include <mrpt/system/os.h>
14 #include <mrpt/system/datetime.h>
15 
16 using namespace mrpt;
17 using namespace mrpt::random;
18 using namespace mrpt::math;
19 using namespace mrpt::utils;
20 using namespace std;
21 
22 // The global instance of CRandomGenerator for single-thread programs:
24 
25 // MT19937 algorithm
26 // http://en.wikipedia.org/wiki/Mersenne_twister
27 // Initialize the generator from a seed
29 {
30  m_MT19937_data.seed_initialized = true;
31  m_MT19937_data.MT[0] = seed;
32  for (uint32_t i=1;i<624;i++)
33  m_MT19937_data.MT[i] = static_cast<uint32_t>( 1812433253 * (m_MT19937_data.MT[i-1] ^ ( m_MT19937_data.MT[i-1] >> 30 )) + i); // 0x6c078965
34 }
35 
36 #if defined(_MSC_VER)
37  #pragma warning(push)
38  #pragma warning(disable:4146)
39 #endif
40 
41 // Code from the implementation by Rick Wagner
42 // http://www-personal.umich.edu/~wagnerr/MersenneTwister.html
43 inline uint32_t hiBit( const uint32_t u ) { return u & 0x80000000UL; }
44 inline uint32_t loBit( const uint32_t u ) { return u & 0x00000001UL; }
45 inline uint32_t loBits( const uint32_t u ) { return u & 0x7fffffffUL; }
46 inline uint32_t mixBits( const uint32_t u, const uint32_t v ) { return hiBit(u) | loBits(v); }
47 inline uint32_t twist( const uint32_t m, const uint32_t s0, const uint32_t s1 ) { return m ^ (mixBits(s0,s1)>>1) ^ (-loBit(s1) & 0x9908b0dfUL); }
48 
49 #if defined(_MSC_VER)
50  #pragma warning(pop)
51 #endif
52 
53 
54 // Generate an array of 624 untempered numbers
56 {
57  if (!m_MT19937_data.seed_initialized) this->randomize();
58 
59  // Code from the implementation by Rick Wagner
60  // http://www-personal.umich.edu/~wagnerr/MersenneTwister.html
61  // and:
62  // http://www.math.sci.hiroshima-u.ac.jp/~m-mat/MT/MT2002/CODES/mt19937ar.c
63  //
64  const int N = 624; // length of state vector
65  const int M = 397; // period parameter
66 
67  uint32_t *p = m_MT19937_data.MT;
68  for( int i = N - M; i--; ++p )
69  *p = twist( p[M], p[0], p[1] );
70  for( int i = M; --i; ++p )
71  *p = twist( p[M-N], p[0], p[1] );
72  *p = twist( p[M-N], p[0], m_MT19937_data.MT[0] );
73 }
74 
76 {
77  uint32_t n1 = drawUniform32bit();
78  uint32_t n2 = drawUniform32bit();
79  return static_cast<uint64_t>(n1) | (static_cast<uint64_t>(n2)<<32);
80 }
81 
82 // MT19937 algorithm
83 // http://en.wikipedia.org/wiki/Mersenne_twister
85 {
86  if (!m_MT19937_data.index)
87  MT19937_generateNumbers();
88 
89  uint32_t y = m_MT19937_data.MT[m_MT19937_data.index];
90  y ^= y >> 11;
91  y ^= (y << 7) & 2636928640U; // 0x9d2c5680
92  y ^= (y << 15) & 4022730752U; // 0xefc60000
93  y ^= (y >> 18);
94 
95  // Wrap index to [0,623].
96  m_MT19937_data.index++;
97  if (m_MT19937_data.index>=624) m_MT19937_data.index=0;
98 
99  return y;
100 }
101 
102 /*---------------------------------------------------------------
103  Randomize
104  ---------------------------------------------------------------*/
106 {
107  MT19937_initializeGenerator(seed);
108  m_MT19937_data.index = 0;
109 }
110 
111 /*---------------------------------------------------------------
112  Randomize
113  ---------------------------------------------------------------*/
115 {
116  MT19937_initializeGenerator( static_cast<uint32_t>(mrpt::system::getCurrentTime()) );
117  m_MT19937_data.index = 0;
118 }
119 
120 /*---------------------------------------------------------------
121  normalizedGaussian
122  This is a routine which has been extracted from page 217 in
123  the Numerical Recipes in C, William H. Press
124  ---------------------------------------------------------------*/
126 {
127  if (!m_std_gauss_set)
128  { /* We don't have an extra deviate handy so */
129  double v1,v2,r;
130  do
131  {
132  v1 = this->drawUniform(-1.0,1.0); /* pick two uniform numbers in */
133  v2 = this->drawUniform(-1.0,1.0); /* square extending from -1 to +1*/
134  /* in each direction, */
135  r = v1 * v1 + v2 * v2; /* see if they are in the unitcircle*/
136  } while (r >= 1.0 || r==0.0); /* and if they are not, try again. */
137  const double fac = std::sqrt(-2.0*log(r)/r);
138  /* Now make the Box-Muller transformation to get two normal deviates.
139  Return one and save the other for next time. */
140  m_std_gauss_next = v1 * fac;
141  m_std_gauss_set = true;
142 
143  if (likelihood)
144  {
145  const double x = v2*fac;
146  *likelihood = 0.39894228040143267794 * exp( -0.5*x*x );
147  return x;
148  }
149  else
150  {
151  return v2*fac;
152  }
153  }
154  else
155  { /* We have an extra deviate handy, */
156  m_std_gauss_set = false; /* so unset the flag, */
157 
158  if (likelihood)
159  {
160  const double x = m_std_gauss_next;
161  *likelihood = 0.39894228040143267794 * exp( -0.5*x*x );
162  return x;
163  }
164  else
165  {
166  return m_std_gauss_next;
167  }
168  }
169 }
170 
171 /** Generates a random definite-positive matrix of the given size, using the formula C = v*v^t + epsilon*I, with "v" being a vector of gaussian random samples.
172 */
174  const size_t dim,
175  const double std_scale,
176  const double diagonal_epsilon)
177 {
178  CMatrixDouble r(dim,1);
179  drawGaussian1DMatrix(r, 0,std_scale);
180  CMatrixDouble cov(dim,dim);
181  cov.multiply_AAt(r); // random semi-definite positive matrix:
182  for (size_t i=0;i<dim;i++) cov(i,i)+=diagonal_epsilon; // make sure it's definite-positive
183  return cov;
184 }
185 
186 
187 /*---------------------------------------------------------------
188  drawGaussianMultivariate
189  ---------------------------------------------------------------*/
190 template <typename T>
192  std::vector<T> &out_result,
194  const std::vector<T>* mean )
195 {
196  ASSERT_(cov.getRowCount() == cov.getColCount() );
197  const size_t dim = cov.getColCount();
198 
199  if (mean) ASSERT_(mean->size()==dim)
200 
202 
203  MRPT_START
204 
205  // Set size of output vector:
206  out_result.clear();
207  out_result.resize(dim,0);
208 
209  /** Computes the eigenvalues/eigenvector decomposition of this matrix,
210  * so that: M = Z · D · Z<sup>T</sup>, where columns in Z are the
211  * eigenvectors and the diagonal matrix D contains the eigenvalues
212  * as diagonal elements, sorted in <i>ascending</i> order.
213  */
214  cov.eigenVectors( Z, D );
215 
216  // Scale eigenvectors with eigenvalues:
217  D = D.array().sqrt().matrix();
218  Z.multiply(Z,D);
219 
220  for (size_t i=0;i<dim;i++)
221  {
222  T rnd = this->drawGaussian1D_normalized();
223  for (size_t d=0;d<dim;d++)
224  out_result[d]+= ( Z.get_unsafe(d,i)* rnd );
225  }
226  if (mean)
227  for (size_t d=0;d<dim;d++) out_result[d]+= (*mean)[d];
228 
230  printf("\nEXCEPTION: Dumping variables for debugging:\n"); \
231  std::cout << "Z:\n" << Z << "D:\n" << D << "Cov:\n" << cov; \
232  try \
233  { \
234  cov.eigenVectors(Z,D); \
235  std::cout << "Original Z:" << Z << "Original D:" << D; \
236  } \
237  catch(...) {}; \
238  );
239 }
240 
241 // Instantiations:
242 template BASE_IMPEXP void CRandomGenerator::drawGaussianMultivariate<double>(std::vector<double> &out_result,const CMatrixTemplateNumeric<double> &cov, const std::vector<double>* mean );
243 template BASE_IMPEXP void CRandomGenerator::drawGaussianMultivariate<float>(std::vector<float> &out_result,const CMatrixTemplateNumeric<float> &cov, const std::vector<float>* mean );
244 
245 
246 
247 
248 
249 namespace mrpt
250 {
251 
252  namespace random
253  {
254 
255  double normalizedGaussian( double *likelihood ) {
256  return randomGenerator.drawGaussian1D_normalized(likelihood);
257  }
258 
259  double RandomNormal( double mean , double std ) {
260  return randomGenerator.drawGaussian1D(mean,std);
261  }
262 
265  }
266 
267  double RandomUni( const double min, const double max)
268  {
269  return min + (max-min)* randomGenerator.drawUniform32bit() * 2.3283064370807973754314699618685e-10; // 0xFFFFFFFF ^ -1
270  }
271 
272  }
273 } // end of namespace
uint32_t drawUniform32bit()
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, in the whole range of 32-bit integers.
const GLdouble * v
Definition: glew.h:1296
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
double RandomNormal(double mean, double std)
uint32_t loBit(const uint32_t u)
#define min(a, b)
#define MRPT_END_WITH_CLEAN_UP(stuff)
mrpt::system::TTimeStamp BASE_IMPEXP getCurrentTime()
Returns the current (UTC) system time.
Definition: datetime.cpp:71
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
GLfloat GLfloat v1
Definition: glew.h:1759
GLfloat GLfloat GLfloat v2
Definition: glew.h:1763
A thred-safe pseudo random number generator, based on an internal MT19937 randomness generator...
double drawGaussian1D_normalized(double *likelihood=NULL)
Generate a normalized (mean=0, std=1) normally distributed sample.
double drawGaussian1D(const double mean, const double std)
Generate a normally distributed pseudo-random number.
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
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
uint32_t RandomUniInt()
GLfloat GLfloat p
Definition: glew.h:10113
double normalizedGaussian(double *likelihood)
uint32_t hiBit(const uint32_t u)
uint64_t drawUniform64bit()
Returns a uniformly distributed pseudo-random number by joining two 32bit numbers from drawUniform32b...
#define MRPT_START
mrpt::math::CMatrixDouble drawDefinitePositiveMatrix(const size_t dim, const double std_scale=1.0, const double diagonal_epsilon=1e-8)
Generates a random definite-positive matrix of the given size, using the formula C = v*v^t + epsilon*...
unsigned __int64 uint64_t
Definition: rptypes.h:52
uint32_t mixBits(const uint32_t u, const uint32_t v)
void randomize()
Randomize the generators, based on current time.
void MT19937_initializeGenerator(const uint32_t &seed)
GLdouble GLdouble GLdouble r
Definition: glew.h:1311
#define ASSERT_(f)
const GLdouble * m
Definition: glew.h:5094
uint32_t twist(const uint32_t m, const uint32_t s0, const uint32_t s1)
double RandomUni(const double min, const double max)
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
void drawGaussianMultivariate(std::vector< T > &out_result, const mrpt::math::CMatrixTemplateNumeric< T > &cov, const std::vector< T > *mean=NULL)
Generate multidimensional random samples according to a given covariance matrix.
unsigned __int32 uint32_t
Definition: rptypes.h:49
uint32_t loBits(const uint32_t u)



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