RandomNumbers.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ioan Sucan, Jonathan Gammell*/
36 
37 // Enable the use of shallow_array_adaptor to create a uBLAS-vector-view of C-style array without copying data
38 #define BOOST_UBLAS_SHALLOW_ARRAY_ADAPTOR
39 
40 #include "ompl/util/RandomNumbers.h"
41 #include "ompl/util/Exception.h"
42 #include "ompl/util/Console.h"
43 #include <mutex>
44 #include <memory>
45 #include <boost/math/constants/constants.hpp>
46 #include <boost/scoped_ptr.hpp>
47 #include <boost/random/uniform_on_sphere.hpp>
48 #include <boost/random/variate_generator.hpp>
49 // For boost::numeric::ublas::shallow_array_adaptor:
50 #include <boost/numeric/ublas/vector.hpp>
51 
53 namespace
54 {
58  class RNGSeedGenerator
59  {
60  public:
61  RNGSeedGenerator()
62  : someSeedsGenerated_(false)
63  , firstSeed_(std::chrono::duration_cast<std::chrono::microseconds>(
64  std::chrono::system_clock::now() - std::chrono::system_clock::time_point::min()).count())
65  , sGen_(firstSeed_)
66  , sDist_(1, 1000000000)
67  {
68  }
69 
70  std::uint_fast32_t firstSeed()
71  {
72  std::lock_guard<std::mutex> slock(rngMutex_);
73  return firstSeed_;
74  }
75 
76  void setSeed(std::uint_fast32_t seed)
77  {
78  std::lock_guard<std::mutex> slock(rngMutex_);
79  if (seed > 0)
80  {
81  if (someSeedsGenerated_)
82  {
83  OMPL_ERROR("Random number generation already started. Changing seed now will not lead to "
84  "deterministic sampling.");
85  }
86  else
87  {
88  // In this case, since no seeds have been generated yet, so we remember this seed as the first one.
89  firstSeed_ = seed;
90  }
91  }
92  else
93  {
94  if (someSeedsGenerated_)
95  {
96  OMPL_WARN("Random generator seed cannot be 0. Ignoring seed.");
97  return;
98  }
99  else
100  {
101  OMPL_WARN("Random generator seed cannot be 0. Using 1 instead.");
102  seed = 1;
103  }
104  }
105  sGen_.seed(seed);
106  }
107 
108  std::uint_fast32_t nextSeed()
109  {
110  std::lock_guard<std::mutex> slock(rngMutex_);
111  someSeedsGenerated_ = true;
112  return sDist_(sGen_);
113  }
114 
115  private:
116  bool someSeedsGenerated_;
117  std::uint_fast32_t firstSeed_;
118  std::mutex rngMutex_;
119  std::ranlux24_base sGen_;
120  std::uniform_int_distribution<> sDist_;
121  };
122 
123  static std::once_flag g_once;
124  static boost::scoped_ptr<RNGSeedGenerator> g_RNGSeedGenerator;
125 
126  void initRNGSeedGenerator()
127  {
128  g_RNGSeedGenerator.reset(new RNGSeedGenerator());
129  }
130 
131  RNGSeedGenerator &getRNGSeedGenerator()
132  {
133  std::call_once(g_once, &initRNGSeedGenerator);
134  return *g_RNGSeedGenerator;
135  }
136 } // namespace
138 
140 class ompl::RNG::SphericalData
141 {
142 public:
144  using container_type_t = boost::numeric::ublas::shallow_array_adaptor<double>;
145 
147  using spherical_dist_t = boost::uniform_on_sphere<double, container_type_t>;
148 
150  using variate_generator_t = boost::variate_generator<std::mt19937 *, spherical_dist_t>;
151 
153  SphericalData(std::mt19937 *generatorPtr) : generatorPtr_(generatorPtr){};
154 
156  container_type_t generate(unsigned int dim)
157  {
158  // Assure that the dimension is in the range of the vector.
159  growVector(dim);
160 
161  // Assure that the dimension is allocated:
162  allocateDimension(dim);
163 
164  // Return the generator
165  return (*dimVector_.at(dim).second)();
166  };
167 
169  void reset()
170  {
171  // Iterate over each dimension
172  for (auto &i : dimVector_)
173  {
174  // Check if the variate_generator is allocated
175  if (bool(i.first) == true)
176  {
177  // It is, reset THE DATA (not the pointer)
178  i.first->reset();
179  }
180  // No else, this is an uninitialized dimension.
181  }
182  };
183 
184 private:
186  using dist_gen_pair_t = std::pair<std::shared_ptr<spherical_dist_t>, std::shared_ptr<variate_generator_t>>;
187 
189  std::vector<dist_gen_pair_t> dimVector_;
190 
192  std::mt19937 *generatorPtr_;
193 
195  void growVector(unsigned int dim)
196  {
197  // Iterate until the index associated with this dimension is in the vector
198  while (dim >= dimVector_.size())
199  {
200  // Create a pair of empty pointers:
201  dimVector_.push_back(dist_gen_pair_t());
202  }
203  };
204 
206  void allocateDimension(unsigned int dim)
207  {
208  // Only do this if unallocated, so check that:
209  if (dimVector_.at(dim).first == nullptr)
210  {
211  // It is not allocated, so....
212  // First construct the distribution
213  dimVector_.at(dim).first = std::make_shared<spherical_dist_t>(dim);
214  // Then the variate generator
215  dimVector_.at(dim).second = std::make_shared<variate_generator_t>(generatorPtr_, *dimVector_.at(dim).first);
216  }
217  // No else, the pointer is already allocated.
218  };
219 };
221 
222 std::uint_fast32_t ompl::RNG::getSeed()
223 {
224  return getRNGSeedGenerator().firstSeed();
225 }
226 
227 void ompl::RNG::setSeed(std::uint_fast32_t seed)
228 {
229  getRNGSeedGenerator().setSeed(seed);
230 }
231 
233  : localSeed_(getRNGSeedGenerator().nextSeed())
234  , generator_(localSeed_)
235  , uniDist_(0, 1)
236  , normalDist_(0, 1)
237  , sphericalDataPtr_(std::make_shared<SphericalData>(&generator_))
238 {
239 }
240 
241 ompl::RNG::RNG(std::uint_fast32_t localSeed)
242  : localSeed_(localSeed)
243  , generator_(localSeed_)
244  , uniDist_(0, 1)
245  , normalDist_(0, 1)
246  , sphericalDataPtr_(std::make_shared<SphericalData>(&generator_))
247 {
248 }
249 
250 void ompl::RNG::setLocalSeed(std::uint_fast32_t localSeed)
251 {
252  // Store the seed
253  localSeed_ = localSeed;
254 
255  // Change the generator's seed
256  generator_.seed(localSeed_);
257 
258  // Reset the distributions used by the variate generators, as they can cache values
259  uniDist_.reset();
260  normalDist_.reset();
261  sphericalDataPtr_->reset();
262 }
263 
264 double ompl::RNG::halfNormalReal(double r_min, double r_max, double focus)
265 {
266  assert(r_min <= r_max);
267 
268  const double mean = r_max - r_min;
269  double v = gaussian(mean, mean / focus);
270 
271  if (v > mean)
272  v = 2.0 * mean - v;
273  double r = v >= 0.0 ? v + r_min : r_min;
274  return r > r_max ? r_max : r;
275 }
276 
277 int ompl::RNG::halfNormalInt(int r_min, int r_max, double focus)
278 {
279  int r = (int)floor(halfNormalReal((double)r_min, (double)(r_max) + 1.0, focus));
280  return (r > r_max) ? r_max : r;
281 }
282 
283 // From: "Uniform Random Rotations", Ken Shoemake, Graphics Gems III,
284 // pg. 124-132
285 void ompl::RNG::quaternion(double value[4])
286 {
287  double x0 = uniDist_(generator_);
288  double r1 = sqrt(1.0 - x0), r2 = sqrt(x0);
289  double t1 = 2.0 * boost::math::constants::pi<double>() * uniDist_(generator_),
290  t2 = 2.0 * boost::math::constants::pi<double>() * uniDist_(generator_);
291  double c1 = cos(t1), s1 = sin(t1);
292  double c2 = cos(t2), s2 = sin(t2);
293  value[0] = s1 * r1;
294  value[1] = c1 * r1;
295  value[2] = s2 * r2;
296  value[3] = c2 * r2;
297 }
298 
299 // From Effective Sampling and Distance Metrics for 3D Rigid Body Path Planning, by James Kuffner, ICRA 2004
300 void ompl::RNG::eulerRPY(double value[3])
301 {
302  value[0] = boost::math::constants::pi<double>() * (-2.0 * uniDist_(generator_) + 1.0);
303  value[1] = acos(1.0 - 2.0 * uniDist_(generator_)) - boost::math::constants::pi<double>() / 2.0;
304  value[2] = boost::math::constants::pi<double>() * (-2.0 * uniDist_(generator_) + 1.0);
305 }
306 
307 void ompl::RNG::uniformNormalVector(unsigned int n, double value[])
308 {
309  // Create a uBLAS-vector-view of the C-style array without copying data
310  SphericalData::container_type_t rVector(n, value);
311 
312  // Generate a random value, the variate_generator is returning a shallow_array_adaptor, which will modify the value
313  // array:
314  rVector = sphericalDataPtr_->generate(n);
315 }
316 
317 // See: http://math.stackexchange.com/a/87238
318 void ompl::RNG::uniformInBall(double r, unsigned int n, double value[])
319 {
320  // Draw a random point on the unit sphere
321  uniformNormalVector(n, value);
322 
323  // Draw a random radius scale
324  double radiusScale = r * std::pow(uniformReal(0.0, 1.0), 1.0 / static_cast<double>(n));
325 
326  // Scale the point on the unit sphere
327  for (unsigned int i = 0u; i < n; ++i)
328  {
329  value[i] *= radiusScale;
330  }
331 }
332 
333 #if OMPL_HAVE_EIGEN3
334 void ompl::RNG::uniformProlateHyperspheroidSurface(const std::shared_ptr<const ProlateHyperspheroid> &phsPtr,
335  double value[])
336 {
337  // Variables
338  // The spherical point as a std::vector
339  std::vector<double> sphere(phsPtr->getDimension());
340 
341  // Get a random point on the sphere
342  uniformNormalVector(phsPtr->getDimension(), &sphere[0]);
343 
344  // Transform to the PHS
345  phsPtr->transform(&sphere[0], value);
346 }
347 
348 void ompl::RNG::uniformProlateHyperspheroid(const std::shared_ptr<const ProlateHyperspheroid> &phsPtr, double value[])
349 {
350  // Variables
351  // The spherical point as a std::vector
352  std::vector<double> sphere(phsPtr->getDimension());
353 
354  // Get a random point in the sphere
355  uniformInBall(1.0, phsPtr->getDimension(), &sphere[0]);
356 
357  // Transform to the PHS
358  phsPtr->transform(&sphere[0], value);
359 }
360 #endif
STL namespace.
void quaternion(double value[4])
Uniform random unit quaternion sampling. The computed value has the order (x,y,z,w). The return variable value is expected to already exist.
RNG()
Constructor. Always sets a different random seed.
void eulerRPY(double value[3])
Uniform random sampling of Euler roll-pitch-yaw angles, each in the range (-pi, pi]. The computed value has the order (roll, pitch, yaw). The return variable value is expected to already exist.
int halfNormalInt(int r_min, int r_max, double focus=3.0)
Generate a random integer using a half-normal distribution. The value is within specified bounds ([r_...
void uniformNormalVector(unsigned int n, double value[])
Uniform random sampling of a unit-length vector. I.e., the surface of an n-ball. The return variable ...
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
double uniformReal(double lower_bound, double upper_bound)
Generate a random real within given bounds: [lower_bound, upper_bound)
Definition: RandomNumbers.h:74
static std::uint_fast32_t getSeed()
Get the seed used to generate the seeds of each RNG instance. Passing the returned value to setSeed()...
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
double halfNormalReal(double r_min, double r_max, double focus=3.0)
Generate a random real using a half-normal distribution. The value is within specified bounds [r_min...
point now()
Get the current time point.
Definition: Time.h:70
void setLocalSeed(std::uint_fast32_t localSeed)
Set the seed used for the instance of a RNG. Use this function to ensure that an instance of an RNG g...
static void setSeed(std::uint_fast32_t seed)
Set the seed used to generate the seeds of each RNG instance. Use this function to ensure the same se...
void uniformInBall(double r, unsigned int n, double value[])
Uniform random sampling of the content of an n-ball, with a radius appropriately distributed between ...
double gaussian(double mean, double stddev)
Generate a random real using a normal distribution with given mean and variance.