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 */
36 
37 #include "ompl/util/RandomNumbers.h"
38 #include "ompl/util/Exception.h"
39 #include "ompl/util/Console.h"
40 #include <boost/random/lagged_fibonacci.hpp>
41 #include <boost/random/uniform_int.hpp>
42 #include <boost/thread/mutex.hpp>
43 #include <boost/thread/once.hpp>
44 #include <boost/scoped_ptr.hpp>
45 #include <boost/date_time/posix_time/posix_time.hpp>
46 #include <boost/math/constants/constants.hpp>
47 
49 namespace
50 {
54  class RNGSeedGenerator
55  {
56  public:
57  RNGSeedGenerator() :
58  someSeedsGenerated_(false),
59  firstSeed_((boost::uint32_t)(boost::posix_time::microsec_clock::universal_time() -
60  boost::posix_time::ptime(boost::date_time::min_date_time)).total_microseconds()),
61  sGen_(firstSeed_),
62  sDist_(1, 1000000000),
63  s_(sGen_, sDist_)
64  {
65  }
66 
67  boost::uint32_t firstSeed()
68  {
69  boost::mutex::scoped_lock slock(rngMutex_);
70  return firstSeed_;
71  }
72 
73  void setSeed(boost::uint32_t seed)
74  {
75  boost::mutex::scoped_lock slock(rngMutex_);
76  if (seed > 0)
77  {
78  if (someSeedsGenerated_)
79  {
80  OMPL_ERROR("Random number generation already started. Changing seed now will not lead to deterministic sampling.");
81  }
82  else
83  {
84  // In this case, since no seeds have been generated yet, so we remember this seed as the first one.
85  firstSeed_ = seed;
86  }
87  }
88  else
89  {
90  if (someSeedsGenerated_)
91  {
92  OMPL_WARN("Random generator seed cannot be 0. Ignoring seed.");
93  return;
94  }
95  else
96  {
97  OMPL_WARN("Random generator seed cannot be 0. Using 1 instead.");
98  seed = 1;
99  }
100  }
101  sGen_.seed(seed);
102  }
103 
104  boost::uint32_t nextSeed()
105  {
106  boost::mutex::scoped_lock slock(rngMutex_);
107  someSeedsGenerated_ = true;
108  return s_();
109  }
110 
111  private:
112  bool someSeedsGenerated_;
113  boost::uint32_t firstSeed_;
114  boost::mutex rngMutex_;
115  boost::lagged_fibonacci607 sGen_;
116  boost::uniform_int<> sDist_;
117  boost::variate_generator<boost::lagged_fibonacci607&, boost::uniform_int<> > s_;
118  };
119 
120  static boost::once_flag g_once = BOOST_ONCE_INIT;
121  static boost::scoped_ptr<RNGSeedGenerator> g_RNGSeedGenerator;
122 
123  void initRNGSeedGenerator()
124  {
125  g_RNGSeedGenerator.reset(new RNGSeedGenerator());
126  }
127 
128  RNGSeedGenerator& getRNGSeedGenerator()
129  {
130  boost::call_once(&initRNGSeedGenerator, g_once);
131  return *g_RNGSeedGenerator;
132  }
133 } // namespace
135 
136 boost::uint32_t ompl::RNG::getSeed()
137 {
138  return getRNGSeedGenerator().firstSeed();
139 }
140 
141 void ompl::RNG::setSeed(boost::uint32_t seed)
142 {
143  getRNGSeedGenerator().setSeed(seed);
144 }
145 
147  generator_(getRNGSeedGenerator().nextSeed()),
148  uniDist_(0, 1),
149  normalDist_(0, 1),
150  uni_(generator_, uniDist_),
151  normal_(generator_, normalDist_)
152 {
153 }
154 
155 double ompl::RNG::halfNormalReal(double r_min, double r_max, double focus)
156 {
157  assert(r_min <= r_max);
158 
159  const double mean = r_max - r_min;
160  double v = gaussian(mean, mean/focus);
161 
162  if (v > mean) v = 2.0 * mean - v;
163  double r = v >= 0.0 ? v + r_min : r_min;
164  return r > r_max ? r_max : r;
165 }
166 
167 int ompl::RNG::halfNormalInt(int r_min, int r_max, double focus)
168 {
169  int r = (int)floor(halfNormalReal((double)r_min, (double)(r_max) + 1.0, focus));
170  return (r > r_max) ? r_max : r;
171 }
172 
173 // From: "Uniform Random Rotations", Ken Shoemake, Graphics Gems III,
174 // pg. 124-132
175 void ompl::RNG::quaternion(double value[4])
176 {
177  double x0 = uni_();
178  double r1 = sqrt(1.0 - x0), r2 = sqrt(x0);
179  double t1 = 2.0 * boost::math::constants::pi<double>() * uni_(), t2 = 2.0 * boost::math::constants::pi<double>() * uni_();
180  double c1 = cos(t1), s1 = sin(t1);
181  double c2 = cos(t2), s2 = sin(t2);
182  value[0] = s1 * r1;
183  value[1] = c1 * r1;
184  value[2] = s2 * r2;
185  value[3] = c2 * r2;
186 }
187 
188 // From Effective Sampling and Distance Metrics for 3D Rigid Body Path Planning, by James Kuffner, ICRA 2004
189 void ompl::RNG::eulerRPY(double value[3])
190 {
191  value[0] = boost::math::constants::pi<double>() * (-2.0 * uni_() + 1.0);
192  value[1] = acos(1.0 - 2.0 * uni_()) - boost::math::constants::pi<double>() / 2.0;
193  value[2] = boost::math::constants::pi<double>() * (-2.0 * uni_() + 1.0);
194 }
static void setSeed(boost::uint32_t seed)
Set the seed for random number generation. Use this function to ensure the same sequence of random nu...
void quaternion(double value[4])
Uniform random unit quaternion sampling. The computed value has the order (x,y,z,w) ...
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)
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_...
static boost::uint32_t getSeed()
Get the seed used for random number generation. Passing the returned value to setSeed() at a subseque...
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
#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...
double gaussian(double mean, double stddev)
Generate a random real using a normal distribution with given mean and variance.
Definition: RandomNumbers.h:94