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
48namespace
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
132class ompl::RNG::SphericalData
133{
134public:
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
172private:
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
208std::uint_fast32_t ompl::RNG::getSeed()
209{
210 return getRNGSeedGenerator().firstSeed();
211}
212
213void 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
225ompl::RNG::RNG(std::uint_fast32_t localSeed)
226 : localSeed_(localSeed), generator_(localSeed_), sphericalDataPtr_(std::make_shared<SphericalData>(&generator_))
227{
228}
229
230void 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
244double 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
257int 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
265void 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
280void 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
287void 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
295void 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
307void 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
321void 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 quaternion(double value[4])
Uniform random unit quaternion sampling. The computed value has the order (x,y,z,w)....
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 eulerRPY(double value[3])
Uniform random sampling of Euler roll-pitch-yaw angles, each in the range (-pi, pi]....
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 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 uniformInBall(double r, std::vector< double > &v)
Uniform random sampling of the content of an n-ball, with a radius appropriately distributed between ...
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...
RNG()
Constructor. Always sets a different random seed.
static std::uint_fast32_t getSeed()
Get the seed used to generate the seeds of each RNG instance. Passing the returned value to setSeed()...
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,...
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...
#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
point now()
Get the current time point.
Definition: Time.h:58
STL namespace.