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 #include "ompl/util/RandomNumbers.h"
38 #include "ompl/util/Exception.h"
39 #include "ompl/util/Console.h"
40 #include <mutex>
41 #include <memory>
42 #include <boost/math/constants/constants.hpp>
43 #include <boost/scoped_ptr.hpp>
44 #include <boost/random/uniform_on_sphere.hpp>
45 #include <boost/random/variate_generator.hpp>
46 
48 namespace
49 {
53  class RNGSeedGenerator
54  {
55  public:
56  RNGSeedGenerator()
57  : firstSeed_(std::chrono::duration_cast<std::chrono::microseconds>(
58  std::chrono::system_clock::now() - std::chrono::system_clock::time_point::min())
59  .count())
60  , sGen_(firstSeed_)
61  , sDist_(1, 1000000000)
62  {
63  }
64 
65  std::uint_fast32_t firstSeed()
66  {
67  std::lock_guard<std::mutex> slock(rngMutex_);
68  return firstSeed_;
69  }
70 
71  void setSeed(std::uint_fast32_t seed)
72  {
73  std::lock_guard<std::mutex> slock(rngMutex_);
74  if (seed > 0)
75  {
76  if (someSeedsGenerated_)
77  {
78  OMPL_ERROR("Random number generation already started. Changing seed now will not lead to "
79  "deterministic sampling.");
80  }
81  else
82  {
83  // In this case, since no seeds have been generated yet, so we remember this seed as the first one.
84  firstSeed_ = seed;
85  }
86  }
87  else
88  {
89  if (someSeedsGenerated_)
90  {
91  OMPL_WARN("Random generator seed cannot be 0. Ignoring seed.");
92  return;
93  }
94  OMPL_WARN("Random generator seed cannot be 0. Using 1 instead.");
95  seed = 1;
96  }
97  sGen_.seed(seed);
98  }
99 
100  std::uint_fast32_t nextSeed()
101  {
102  std::lock_guard<std::mutex> slock(rngMutex_);
103  someSeedsGenerated_ = true;
104  return sDist_(sGen_);
105  }
106 
107  private:
108  bool someSeedsGenerated_{false};
109  std::uint_fast32_t firstSeed_;
110  std::mutex rngMutex_;
111  std::ranlux24_base sGen_;
112  std::uniform_int_distribution<> sDist_;
113  };
114 
115  std::once_flag g_once;
116  boost::scoped_ptr<RNGSeedGenerator> g_RNGSeedGenerator;
117 
118  void initRNGSeedGenerator()
119  {
120  g_RNGSeedGenerator.reset(new RNGSeedGenerator());
121  }
122 
123  RNGSeedGenerator &getRNGSeedGenerator()
124  {
125  std::call_once(g_once, &initRNGSeedGenerator);
126  return *g_RNGSeedGenerator;
127  }
128 } // namespace
130 
132 class ompl::RNG::SphericalData
133 {
134 public:
136  using container_type_t = std::vector<double>;
137 
139  using spherical_dist_t = boost::uniform_on_sphere<double, container_type_t>;
140 
142  using variate_generator_t = boost::variate_generator<std::mt19937 *, spherical_dist_t>;
143 
145  SphericalData(std::mt19937 *generatorPtr) : generatorPtr_(generatorPtr){};
146 
148  container_type_t generate(unsigned int dim)
149  {
150  // Assure that the dimension is in the range of the vector.
151  growVector(dim);
152 
153  // Assure that the dimension is allocated:
154  allocateDimension(dim);
155 
156  // Return the generator
157  return (*dimVector_.at(dim).second)();
158  };
159 
161  void reset()
162  {
163  // Iterate over each dimension
164  for (auto &i : dimVector_)
165  // Check if the variate_generator is allocated
166  if (bool(i.first))
167  // It is, reset THE DATA (not the pointer)
168  i.first->reset();
169  // No else, this is an uninitialized dimension.
170  };
171 
172 private:
174  using dist_gen_pair_t = std::pair<std::shared_ptr<spherical_dist_t>, std::shared_ptr<variate_generator_t>>;
175 
177  std::vector<dist_gen_pair_t> dimVector_;
178 
180  std::mt19937 *generatorPtr_;
181 
183  void growVector(unsigned int dim)
184  {
185  // Iterate until the index associated with this dimension is in the vector
186  while (dim >= dimVector_.size())
187  // Create a pair of empty pointers:
188  dimVector_.emplace_back();
189  };
190 
192  void allocateDimension(unsigned int dim)
193  {
194  // Only do this if unallocated, so check that:
195  if (dimVector_.at(dim).first == nullptr)
196  {
197  // It is not allocated, so....
198  // First construct the distribution
199  dimVector_.at(dim).first = std::make_shared<spherical_dist_t>(dim);
200  // Then the variate generator
201  dimVector_.at(dim).second = std::make_shared<variate_generator_t>(generatorPtr_, *dimVector_.at(dim).first);
202  }
203  // No else, the pointer is already allocated.
204  };
205 };
207 
208 std::uint_fast32_t ompl::RNG::getSeed()
209 {
210  return getRNGSeedGenerator().firstSeed();
211 }
212 
213 void ompl::RNG::setSeed(std::uint_fast32_t seed)
214 {
215  getRNGSeedGenerator().setSeed(seed);
216 }
217 
219  : localSeed_(getRNGSeedGenerator().nextSeed())
220  , generator_(localSeed_)
221  , sphericalDataPtr_(std::make_shared<SphericalData>(&generator_))
222 {
223 }
224 
225 ompl::RNG::RNG(std::uint_fast32_t localSeed)
226  : localSeed_(localSeed), generator_(localSeed_), sphericalDataPtr_(std::make_shared<SphericalData>(&generator_))
227 {
228 }
229 
230 void ompl::RNG::setLocalSeed(std::uint_fast32_t localSeed)
231 {
232  // Store the seed
233  localSeed_ = localSeed;
234 
235  // Change the generator's seed
236  generator_.seed(localSeed_);
237 
238  // Reset the distributions used by the variate generators, as they can cache values
239  uniDist_.reset();
240  normalDist_.reset();
241  sphericalDataPtr_->reset();
242 }
243 
244 double ompl::RNG::halfNormalReal(double r_min, double r_max, double focus)
245 {
246  assert(r_min <= r_max);
247 
248  const double mean = r_max - r_min;
249  double v = gaussian(mean, mean / focus);
250 
251  if (v > mean)
252  v = 2.0 * mean - v;
253  double r = v >= 0.0 ? v + r_min : r_min;
254  return r > r_max ? r_max : r;
255 }
256 
257 int ompl::RNG::halfNormalInt(int r_min, int r_max, double focus)
258 {
259  auto r = (int)floor(halfNormalReal((double)r_min, (double)(r_max) + 1.0, focus));
260  return (r > r_max) ? r_max : r;
261 }
262 
263 // From: "Uniform Random Rotations", Ken Shoemake, Graphics Gems III,
264 // pg. 124-132
265 void ompl::RNG::quaternion(double value[4])
266 {
267  double x0 = uniDist_(generator_);
268  double r1 = sqrt(1.0 - x0), r2 = sqrt(x0);
269  double t1 = 2.0 * boost::math::constants::pi<double>() * uniDist_(generator_),
270  t2 = 2.0 * boost::math::constants::pi<double>() * uniDist_(generator_);
271  double c1 = cos(t1), s1 = sin(t1);
272  double c2 = cos(t2), s2 = sin(t2);
273  value[0] = s1 * r1;
274  value[1] = c1 * r1;
275  value[2] = s2 * r2;
276  value[3] = c2 * r2;
277 }
278 
279 // From Effective Sampling and Distance Metrics for 3D Rigid Body Path Planning, by James Kuffner, ICRA 2004
280 void ompl::RNG::eulerRPY(double value[3])
281 {
282  value[0] = boost::math::constants::pi<double>() * (-2.0 * uniDist_(generator_) + 1.0);
283  value[1] = acos(1.0 - 2.0 * uniDist_(generator_)) - boost::math::constants::pi<double>() / 2.0;
284  value[2] = boost::math::constants::pi<double>() * (-2.0 * uniDist_(generator_) + 1.0);
285 }
286 
287 void ompl::RNG::uniformNormalVector(std::vector<double> &v)
288 {
289  // Generate a random value, the variate_generator is returning a shallow_array_adaptor, which will modify the value
290  // array:
291  v = sphericalDataPtr_->generate(v.size());
292 }
293 
294 // See: http://math.stackexchange.com/a/87238
295 void ompl::RNG::uniformInBall(double r, std::vector<double> &v)
296 {
297  // Draw a random point on the unit sphere
298  uniformNormalVector(v);
299 
300  // Draw a random radius scale
301  double radiusScale = r * std::pow(uniformReal(0.0, 1.0), 1.0 / static_cast<double>(v.size()));
302 
303  // Scale the point on the unit sphere
304  std::transform(v.begin(), v.end(), v.begin(), [radiusScale](double x) { return radiusScale * x; });
305 }
306 
307 void ompl::RNG::uniformProlateHyperspheroidSurface(const std::shared_ptr<const ProlateHyperspheroid> &phsPtr,
308  double value[])
309 {
310  // Variables
311  // The spherical point as a std::vector
312  std::vector<double> sphere(phsPtr->getDimension());
313 
314  // Get a random point on the sphere
315  uniformNormalVector(sphere);
316 
317  // Transform to the PHS
318  phsPtr->transform(&sphere[0], value);
319 }
320 
321 void ompl::RNG::uniformProlateHyperspheroid(const std::shared_ptr<const ProlateHyperspheroid> &phsPtr, double value[])
322 {
323  // Variables
324  // The spherical point as a std::vector
325  std::vector<double> sphere(phsPtr->getDimension());
326 
327  // Get a random point in the sphere
328  uniformInBall(1.0, sphere);
329 
330  // Transform to the PHS
331  phsPtr->transform(&sphere[0], value);
332 }
void uniformNormalVector(std::vector< double > &v)
Uniform random sampling of a unit-length vector. I.e., the surface of an n-ball. The return variable ...
void uniformProlateHyperspheroidSurface(const std::shared_ptr< const ProlateHyperspheroid > &phsPtr, double value[])
Uniform random sampling of the surface of a prolate hyperspheroid, a special symmetric type of n-dime...
point now()
Get the current time point.
Definition: Time.h:122
void transform(const double sphere[], double phs[]) const
Transform a point from a sphere to PHS. The return variable phs is expected to already exist.
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,...
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...
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 quaternion(double value[4])
Uniform random unit quaternion sampling. The computed value has the order (x,y,z,w)....
void uniformInBall(double r, std::vector< double > &v)
Uniform random sampling of the content of an n-ball, with a radius appropriately distributed between ...
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
unsigned int getDimension() const
The state dimension of the PHS.
void uniformProlateHyperspheroid(const std::shared_ptr< const ProlateHyperspheroid > &phsPtr, double value[])
Uniform random sampling of a prolate hyperspheroid, a special symmetric type of n-dimensional ellipse...
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
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...
void eulerRPY(double value[3])
Uniform random sampling of Euler roll-pitch-yaw angles, each in the range (-pi, pi]....
RNG()
Constructor. Always sets a different random seed.