Commit b16d32cf authored by panjia1983's avatar panjia1983
Browse files

vec nf

parent 6330ad3f
#ifndef FCL_MATH_SAMPLING_H
#define FCL_MATH_SAMPLING_H
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/assign/list_of.hpp>
#include "fcl/math/vec_nf.h"
#include "fcl/math/transform.h"
namespace fcl
{
/// @brief Random number generation. An instance of this class
/// cannot be used by multiple threads at once (member functions
/// are not const). However, the constructor is thread safe and
/// different instances can be used safely in any number of
/// threads. It is also guaranteed that all created instances will
/// have a different random seed.
class RNG
{
public:
/// @brief Constructor. Always sets a different random seed
RNG();
/// @brief Generate a random real between 0 and 1
double uniform01()
{
return uni_();
}
/// @brief Generate a random real within given bounds: [\e lower_bound, \e upper_bound)
double uniformReal(double lower_bound, double upper_bound)
{
assert(lower_bound <= upper_bound);
return (upper_bound - lower_bound) * uni_() + lower_bound;
}
/// @brief Generate a random integer within given bounds: [\e lower_bound, \e upper_bound]
int uniformInt(int lower_bound, int upper_bound)
{
int r = (int)floor(uniformReal((double)lower_bound, (double)(upper_bound) + 1.0));
return (r > upper_bound) ? upper_bound : r;
}
/// @brief Generate a random boolean
bool uniformBool()
{
return uni_() <= 0.5;
}
/// @brief Generate a random real using a normal distribution with mean 0 and variance 1
double gaussian01()
{
return normal_();
}
/// @brief Generate a random real using a normal distribution with given mean and variance
double gaussian(double mean, double stddev)
{
return normal_() * stddev + mean;
}
/// @brief Generate a random real using a half-normal distribution. The value is within specified bounds [\e r_min, \e r_max], but with a bias towards \e r_max. The function is implemended using a Gaussian distribution with mean at \e r_max - \e r_min. The distribution is 'folded' around \e r_max axis towards \e r_min. The variance of the distribution is (\e r_max - \e r_min) / \e focus. The higher the focus, the more probable it is that generated numbers are close to \e r_max.
double halfNormalReal(double r_min, double r_max, double focus = 3.0);
/// @brief Generate a random integer using a half-normal distribution. The value is within specified bounds ([\e r_min, \e r_max]), but with a bias towards \e r_max. The function is implemented on top of halfNormalReal()
int halfNormalInt(int r_min, int r_max, double focus = 3.0);
/// @brief Uniform random unit quaternion sampling. The computed value has the order (x,y,z,w)
void quaternion(double value[4]);
/// @brief Uniform random sampling of Euler roll-pitch-yaw angles, each in the range [-pi, pi). The computed value has the order (roll, pitch, yaw) */
void eulerRPY(double value[3]);
/// @brief Uniform random sample on a disk with radius from r_min to r_max
void disk(double r_min, double r_max, double& x, double& y);
/// @brief Uniform random sample in a ball with radius from r_min to r_max
void ball(double r_min, double r_max, double& x, double& y, double& z);
/// @brief Set the seed for random number generation. Use this function to ensure the same sequence of random numbers is generated.
static void setSeed(boost::uint32_t seed);
/// @brief Get the seed used for random number generation. Passing the returned value to setSeed() at a subsequent execution of the code will ensure deterministic (repeatable) behaviour. Useful for debugging.
static boost::uint32_t getSeed();
private:
boost::mt19937 generator_;
boost::uniform_real<> uniDist_;
boost::normal_distribution<> normalDist_;
boost::variate_generator<boost::mt19937&, boost::uniform_real<> > uni_;
boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > normal_;
};
class SamplerBase
{
public:
mutable RNG rng;
};
template<std::size_t N>
class SamplerR : public SamplerBase
{
public:
SamplerR() {}
SamplerR(const Vecnf<N>& lower_bound_,
const Vecnf<N>& upper_bound_) : lower_bound(lower_bound_),
upper_bound(upper_bound_)
{
}
void setBound(const Vecnf<N>& lower_bound_,
const Vecnf<N>& upper_bound_)
{
lower_bound = lower_bound_;
upper_bound = upper_bound_;
}
void getBound(Vecnf<N>& lower_bound_,
Vecnf<N>& upper_bound_) const
{
lower_bound_ = lower_bound;
upper_bound_ = upper_bound;
}
Vecnf<N> sample() const
{
Vecnf<N> q;
for(std::size_t i = 0; i < N; ++i)
{
q[i] = rng.uniformReal(lower_bound[i], upper_bound[i]);
}
return q;
}
private:
Vecnf<N> lower_bound;
Vecnf<N> upper_bound;
};
class SamplerSE2 : public SamplerBase
{
public:
SamplerSE2() {}
SamplerSE2(const Vecnf<2>& lower_bound_,
const Vecnf<2>& upper_bound_) : lower_bound(lower_bound_),
upper_bound(upper_bound_)
{}
SamplerSE2(FCL_REAL x_min, FCL_REAL x_max,
FCL_REAL y_min, FCL_REAL y_max) : lower_bound(boost::assign::list_of<FCL_REAL>(x_min)(y_min).convert_to_container<std::vector<FCL_REAL> >()),
upper_bound(boost::assign::list_of<FCL_REAL>(x_max)(y_max).convert_to_container<std::vector<FCL_REAL> >())
{}
void setBound(const Vecnf<2>& lower_bound_,
const Vecnf<2>& upper_bound_)
{
lower_bound = lower_bound_;
upper_bound = upper_bound_;
}
void getBound(Vecnf<2>& lower_bound_,
Vecnf<2>& upper_bound_) const
{
lower_bound_ = lower_bound;
upper_bound_ = upper_bound;
}
Vecnf<3> sample() const
{
Vecnf<3> q;
q[0] = rng.uniformReal(lower_bound[0], lower_bound[1]);
q[1] = rng.uniformReal(lower_bound[1], lower_bound[2]);
q[2] = rng.uniformReal(-boost::math::constants::pi<FCL_REAL>(),
boost::math::constants::pi<FCL_REAL>());
return q;
}
protected:
Vecnf<2> lower_bound;
Vecnf<2> upper_bound;
};
class SamplerSE2_disk : public SamplerBase
{
public:
SamplerSE2_disk() {}
SamplerSE2_disk(FCL_REAL cx, FCL_REAL cy,
FCL_REAL r1, FCL_REAL r2,
FCL_REAL crefx, FCL_REAL crefy)
{
setBound(cx, cy, r1, r2, crefx, crefy);
}
void setBound(FCL_REAL cx, FCL_REAL cy,
FCL_REAL r1, FCL_REAL r2,
FCL_REAL crefx, FCL_REAL crefy)
{
c[0] = cx; c[1] = cy;
cref[0] = crefx; cref[1] = crefy;
r_min = r1;
r_max = r2;
}
Vecnf<3> sample() const
{
Vecnf<3> q;
FCL_REAL x, y;
rng.disk(r_min, r_max, x, y);
q[0] = x + c[0] - cref[0];
q[1] = y + c[1] - cref[1];
q[2] = rng.uniformReal(-boost::math::constants::pi<FCL_REAL>(),
boost::math::constants::pi<FCL_REAL>());
return q;
}
protected:
FCL_REAL c[2];
FCL_REAL cref[2];
FCL_REAL r_min, r_max;
};
class SamplerSE3Euler : public SamplerBase
{
public:
SamplerSE3Euler() {}
SamplerSE3Euler(const Vecnf<3>& lower_bound_,
const Vecnf<3>& upper_bound_) : lower_bound(lower_bound_),
upper_bound(upper_bound_)
{}
SamplerSE3Euler(const Vec3f& lower_bound_,
const Vec3f& upper_bound_) : lower_bound(boost::assign::list_of<FCL_REAL>(lower_bound_[0])(lower_bound_[1])(lower_bound_[2]).convert_to_container<std::vector<FCL_REAL> >()),
upper_bound(boost::assign::list_of<FCL_REAL>(upper_bound_[0])(upper_bound_[1])(upper_bound_[2]).convert_to_container<std::vector<FCL_REAL> >())
{}
void setBound(const Vecnf<3>& lower_bound_,
const Vecnf<3>& upper_bound_)
{
lower_bound = lower_bound_;
upper_bound = upper_bound_;
}
void setBound(const Vec3f& lower_bound_,
const Vec3f& upper_bound_)
{
lower_bound = Vecnf<3>(boost::assign::list_of<FCL_REAL>(lower_bound_[0])(lower_bound_[1])(lower_bound_[2]).convert_to_container<std::vector<FCL_REAL> >());
upper_bound = Vecnf<3>(boost::assign::list_of<FCL_REAL>(upper_bound_[0])(upper_bound_[1])(upper_bound_[2]).convert_to_container<std::vector<FCL_REAL> >());
}
void getBound(Vecnf<3>& lower_bound_,
Vecnf<3>& upper_bound_) const
{
lower_bound_ = lower_bound;
upper_bound_ = upper_bound;
}
void getBound(Vec3f& lower_bound_,
Vec3f& upper_bound_) const
{
lower_bound_ = Vec3f(lower_bound[0], lower_bound[1], lower_bound[2]);
upper_bound_ = Vec3f(upper_bound[0], upper_bound[1], upper_bound[2]);
}
Vecnf<6> sample() const
{
Vecnf<6> q;
q[0] = rng.uniformReal(lower_bound[0], upper_bound[0]);
q[1] = rng.uniformReal(lower_bound[1], upper_bound[1]);
q[2] = rng.uniformReal(lower_bound[2], upper_bound[2]);
FCL_REAL s[4];
rng.quaternion(s);
Quaternion3f quat(s[0], s[1], s[2], s[3]);
FCL_REAL a, b, c;
quat.toEuler(a, b, c);
q[3] = a;
q[4] = b;
q[5] = c;
return q;
}
protected:
Vecnf<3> lower_bound;
Vecnf<3> upper_bound;
};
class SamplerSE3Quat : public SamplerBase
{
public:
SamplerSE3Quat() {}
SamplerSE3Quat(const Vecnf<3>& lower_bound_,
const Vecnf<3>& upper_bound_) : lower_bound(lower_bound_),
upper_bound(upper_bound_)
{}
SamplerSE3Quat(const Vec3f& lower_bound_,
const Vec3f& upper_bound_) : lower_bound(boost::assign::list_of<FCL_REAL>(lower_bound_[0])(lower_bound_[1])(lower_bound_[2]).convert_to_container<std::vector<FCL_REAL> >()),
upper_bound(boost::assign::list_of<FCL_REAL>(upper_bound_[0])(upper_bound_[1])(upper_bound_[2]).convert_to_container<std::vector<FCL_REAL> >())
{}
void setBound(const Vecnf<3>& lower_bound_,
const Vecnf<3>& upper_bound_)
{
lower_bound = lower_bound_;
upper_bound = upper_bound_;
}
void setBound(const Vec3f& lower_bound_,
const Vec3f& upper_bound_)
{
lower_bound = Vecnf<3>(boost::assign::list_of<FCL_REAL>(lower_bound_[0])(lower_bound_[1])(lower_bound_[2]).convert_to_container<std::vector<FCL_REAL> >());
upper_bound = Vecnf<3>(boost::assign::list_of<FCL_REAL>(upper_bound_[0])(upper_bound_[1])(upper_bound_[2]).convert_to_container<std::vector<FCL_REAL> >());
}
void getBound(Vecnf<3>& lower_bound_,
Vecnf<3>& upper_bound_) const
{
lower_bound_ = lower_bound;
upper_bound_ = upper_bound;
}
void getBound(Vec3f& lower_bound_,
Vec3f& upper_bound_) const
{
lower_bound_ = Vec3f(lower_bound[0], lower_bound[1], lower_bound[2]);
upper_bound_ = Vec3f(upper_bound[0], upper_bound[1], upper_bound[2]);
}
Vecnf<7> sample() const
{
Vecnf<7> q;
q[0] = rng.uniformReal(lower_bound[0], upper_bound[0]);
q[1] = rng.uniformReal(lower_bound[1], upper_bound[1]);
q[2] = rng.uniformReal(lower_bound[2], upper_bound[2]);
FCL_REAL s[4];
rng.quaternion(s);
q[3] = s[0];
q[4] = s[1];
q[5] = s[2];
q[6] = s[3];
return q;
}
protected:
Vecnf<3> lower_bound;
Vecnf<3> upper_bound;
};
class SamplerSE3Euler_ball : public SamplerBase
{
public:
SamplerSE3Euler_ball() {}
SamplerSE3Euler_ball(const Vec3f& c_,
FCL_REAL r1, FCL_REAL r2,
const Vec3f& c_ref)
{
setBound(c_, r1, r2, c_ref);
}
SamplerSE3Euler_ball(const Vec3f& c_,
FCL_REAL r_min_, FCL_REAL r_max_)
{
setBound(c_, r_min_, r_max_);
}
void setBound(const Vec3f& c_,
FCL_REAL r1, FCL_REAL r2,
const Vec3f& cref_)
{
c = c_;
FCL_REAL cref_len = cref_.length();
r_min = std::max<FCL_REAL>(cref_len - (r1 + r2), 0);
r_max = cref_len + r1 + r2;
}
void setBound(const Vec3f& c_,
FCL_REAL r_min_, FCL_REAL r_max_)
{
c = c_;
r_min = r_min_;
r_max = r_max_;
}
void getBound(Vec3f& c_,
FCL_REAL& r_min_, FCL_REAL& r_max_) const
{
c_ = c;
r_min_ = r_min;
r_max_ = r_max;
}
Vecnf<6> sample() const
{
Vecnf<6> q;
FCL_REAL x, y, z;
rng.ball(r_min, r_max, x, y, z);
q[0] = x;
q[1] = y;
q[2] = z;
FCL_REAL s[4];
rng.quaternion(s);
Quaternion3f quat(s[0], s[1], s[2], s[3]);
FCL_REAL a, b, c;
quat.toEuler(a, b, c);
q[3] = a;
q[4] = b;
q[5] = c;
return q;
}
protected:
Vec3f c;
FCL_REAL r_min, r_max;
};
class SamplerSE3Quat_ball : public SamplerBase
{
public:
SamplerSE3Quat_ball() {}
SamplerSE3Quat_ball(const Vec3f& c_,
FCL_REAL r1, FCL_REAL r2,
const Vec3f& c_ref)
{
setBound(c_, r1, r2, c_ref);
}
SamplerSE3Quat_ball(const Vec3f& c_,
FCL_REAL r_min_, FCL_REAL r_max_)
{
setBound(c_, r_min_, r_max_);
}
void setBound(const Vec3f& c_,
FCL_REAL r1, FCL_REAL r2,
const Vec3f& cref_)
{
c = c_;
FCL_REAL cref_len = cref_.length();
r_min = std::max<FCL_REAL>(cref_len - (r1 + r2), 0);
r_max = cref_len + r1 + r2;
}
void setBound(const Vec3f& c_,
FCL_REAL r_min_, FCL_REAL r_max_)
{
c = c_;
r_min = r_min_;
r_max = r_max_;
}
void getBound(Vec3f& c_,
FCL_REAL& r_min_, FCL_REAL& r_max_) const
{
c_ = c;
r_min_ = r_min;
r_max_ = r_max;
}
Vecnf<7> sample() const
{
Vecnf<7> q;
FCL_REAL x, y, z;
rng.ball(r_min, r_max, x, y, z);
q[0] = x;
q[1] = y;
q[2] = z;
FCL_REAL s[4];
rng.quaternion(s);
q[3] = s[0];
q[4] = s[1];
q[5] = s[2];
q[6] = s[3];
return q;
}
protected:
Vec3f c;
FCL_REAL r_min, r_max;
};
}
#endif
#ifndef FCL_MATH_VEC_NF_H
#define FCL_MATH_VEC_NF_H
#include <cmath>
#include <iostream>
#include <limits>
#include <vector>
#include <boost/array.hpp>
#include <cstdarg>
#include "fcl/data_types.h"
namespace fcl
{
template<typename T, std::size_t N>
class Vec_n
{
public:
Vec_n()
{
data.resize(N, 0);
}
template<std::size_t M>
Vec_n(const Vec_n<T, M>& data_)
{
std::size_t n = std::min(M, N);
for(std::size_t i = 0; i < n; ++i)
data[i] = data_[i];
}
Vec_n(const std::vector<T>& data_)
{
data.resize(N, 0);
std::size_t n = std::min(data_.size(), N);
for(std::size_t i = 0; i < n; ++i)
data[i] = data_[i];
}
bool operator == (const Vec_n<T, N>& other) const
{
for(std::size_t i = 0; i < N; ++i)
{
if(data[i] != other[i]) return false;
}
return true;
}
bool operator != (const Vec_n<T, N>& other) const
{
for(std::size_t i = 0; i < N; ++i)
{
if(data[i] != other[i]) return true;
}
return false;
}
std::size_t dim() const
{
return N;
}
void setData(const std::vector<T>& data_)
{
std::size_t n = std::min(data.size(), N);
for(std::size_t i = 0; i < n; ++i)
data[i] = data_[i];
}
T operator [] (std::size_t i) const
{
return data[i];
}
T& operator [] (std::size_t i)
{
return data[i];
}
Vec_n<T, N> operator + (const Vec_n<T, N>& other) const
{
Vec_n<T, N> result;
for(std::size_t i = 0; i < N; ++i)
result[i] = data[i] + other[i];