38 #define BOOST_UBLAS_SHALLOW_ARRAY_ADAPTOR 40 #include "ompl/util/RandomNumbers.h" 41 #include "ompl/util/Exception.h" 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> 50 #include <boost/numeric/ublas/vector.hpp> 58 class RNGSeedGenerator
62 : firstSeed_(
std::chrono::duration_cast<
std::chrono::microseconds>(
63 std::chrono::system_clock::
now() -
std::chrono::system_clock::time_point::min()).count())
65 , sDist_(1, 1000000000)
69 std::uint_fast32_t firstSeed()
71 std::lock_guard<std::mutex> slock(rngMutex_);
75 void setSeed(std::uint_fast32_t seed)
77 std::lock_guard<std::mutex> slock(rngMutex_);
80 if (someSeedsGenerated_)
82 OMPL_ERROR(
"Random number generation already started. Changing seed now will not lead to " 83 "deterministic sampling.");
93 if (someSeedsGenerated_)
95 OMPL_WARN(
"Random generator seed cannot be 0. Ignoring seed.");
98 OMPL_WARN(
"Random generator seed cannot be 0. Using 1 instead.");
104 std::uint_fast32_t nextSeed()
106 std::lock_guard<std::mutex> slock(rngMutex_);
107 someSeedsGenerated_ =
true;
108 return sDist_(sGen_);
112 bool someSeedsGenerated_{
false};
113 std::uint_fast32_t firstSeed_;
114 std::mutex rngMutex_;
115 std::ranlux24_base sGen_;
116 std::uniform_int_distribution<> sDist_;
119 std::once_flag g_once;
120 boost::scoped_ptr<RNGSeedGenerator> g_RNGSeedGenerator;
122 void initRNGSeedGenerator()
124 g_RNGSeedGenerator.reset(
new RNGSeedGenerator());
127 RNGSeedGenerator &getRNGSeedGenerator()
129 std::call_once(g_once, &initRNGSeedGenerator);
130 return *g_RNGSeedGenerator;
136 class ompl::RNG::SphericalData
140 using container_type_t = boost::numeric::ublas::shallow_array_adaptor<double>;
143 using spherical_dist_t = boost::uniform_on_sphere<double, container_type_t>;
146 using variate_generator_t = boost::variate_generator<std::mt19937 *, spherical_dist_t>;
149 SphericalData(std::mt19937 *generatorPtr) : generatorPtr_(generatorPtr){};
152 container_type_t generate(
unsigned int dim)
158 allocateDimension(dim);
161 return (*dimVector_.at(dim).second)();
168 for (
auto &i : dimVector_)
182 using dist_gen_pair_t = std::pair<std::shared_ptr<spherical_dist_t>, std::shared_ptr<variate_generator_t>>;
185 std::vector<dist_gen_pair_t> dimVector_;
188 std::mt19937 *generatorPtr_;
191 void growVector(
unsigned int dim)
194 while (dim >= dimVector_.size())
197 dimVector_.emplace_back();
202 void allocateDimension(
unsigned int dim)
205 if (dimVector_.at(dim).first ==
nullptr)
209 dimVector_.at(dim).first = std::make_shared<spherical_dist_t>(dim);
211 dimVector_.at(dim).second = std::make_shared<variate_generator_t>(generatorPtr_, *dimVector_.at(dim).first);
220 return getRNGSeedGenerator().firstSeed();
225 getRNGSeedGenerator().setSeed(seed);
229 : localSeed_(getRNGSeedGenerator().nextSeed())
230 , generator_(localSeed_)
231 , sphericalDataPtr_(
std::make_shared<SphericalData>(&generator_))
236 : localSeed_(localSeed)
237 , generator_(localSeed_)
238 , sphericalDataPtr_(
std::make_shared<SphericalData>(&generator_))
245 localSeed_ = localSeed;
248 generator_.seed(localSeed_);
253 sphericalDataPtr_->reset();
258 assert(r_min <= r_max);
260 const double mean = r_max - r_min;
261 double v = gaussian(mean, mean / focus);
265 double r = v >= 0.0 ? v + r_min : r_min;
266 return r > r_max ? r_max : r;
271 auto r = (int)floor(halfNormalReal((
double)r_min, (
double)(r_max) + 1.0, focus));
272 return (r > r_max) ? r_max : r;
279 double x0 = uniDist_(generator_);
280 double r1 = sqrt(1.0 - x0), r2 = sqrt(x0);
281 double t1 = 2.0 * boost::math::constants::pi<double>() * uniDist_(generator_),
282 t2 = 2.0 * boost::math::constants::pi<double>() * uniDist_(generator_);
283 double c1 = cos(t1), s1 = sin(t1);
284 double c2 = cos(t2), s2 = sin(t2);
294 value[0] = boost::math::constants::pi<double>() * (-2.0 * uniDist_(generator_) + 1.0);
295 value[1] = acos(1.0 - 2.0 * uniDist_(generator_)) - boost::math::constants::pi<double>() / 2.0;
296 value[2] = boost::math::constants::pi<double>() * (-2.0 * uniDist_(generator_) + 1.0);
302 SphericalData::container_type_t rVector(n, value);
306 rVector = sphericalDataPtr_->generate(n);
313 uniformNormalVector(n, value);
316 double radiusScale = r * std::pow(uniformReal(0.0, 1.0), 1.0 / static_cast<double>(n));
319 for (
unsigned int i = 0u; i < n; ++i)
321 value[i] *= radiusScale;
326 void ompl::RNG::uniformProlateHyperspheroidSurface(
const std::shared_ptr<const ProlateHyperspheroid> &phsPtr,
331 std::vector<double> sphere(phsPtr->getDimension());
334 uniformNormalVector(phsPtr->getDimension(), &sphere[0]);
337 phsPtr->transform(&sphere[0], value);
340 void ompl::RNG::uniformProlateHyperspheroid(
const std::shared_ptr<const ProlateHyperspheroid> &phsPtr,
double value[])
344 std::vector<double> sphere(phsPtr->getDimension());
347 uniformInBall(1.0, phsPtr->getDimension(), &sphere[0]);
350 phsPtr->transform(&sphere[0], value);
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.
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.
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.
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 ...