void ompl::base::SO3StateSampler::sampleGaussian(State *state, const State * mean, const double stdDev)
{
// CDF of N(0, 1.17) at -pi/4 is approx. .25, so there's .25 probability
// weight in each tail. Since the maximum distance in SO(3) is pi/2, we're
// essentially as likely to sample a state within distance [0, pi/4] as
// within distance [pi/4, pi/2]. With most weight in the tails (that wrap
// around in case of quaternions) we might as well sample uniformly.
if (stdDev > 1.17)
{
sampleUniform(state);
return;
}
double x = rng_.gaussian(0, stdDev), y = rng_.gaussian(0, stdDev), z = rng_.gaussian(0, stdDev),
theta = std::sqrt(x*x + y*y + z*z);
if (theta < std::numeric_limits<double>::epsilon())
space_->copyState(state, mean);
else
{
SO3StateSpace::StateType q,
*qs = static_cast<SO3StateSpace::StateType*>(state);
const SO3StateSpace::StateType *qmu = static_cast<const SO3StateSpace::StateType*>(mean);
double s = sin(theta / 2) / theta;
q.w = cos(theta / 2);
q.x = s * x;
q.y = s * y;
q.z = s * z;
quaternionProduct(*qs, *qmu, q);
}
}
void ompl::base::SO3StateSampler::sampleUniformNear(State *state, const State *near, const double distance)
{
if (distance >= .25 * boost::math::constants::pi<double>())
{
sampleUniform(state);
return;
}
double d = rng_.uniform01();
SO3StateSpace::StateType q,
*qs = static_cast<SO3StateSpace::StateType*>(state);
const SO3StateSpace::StateType *qnear = static_cast<const SO3StateSpace::StateType*>(near);
computeAxisAngle(q, rng_.gaussian01(), rng_.gaussian01(), rng_.gaussian01(), 2.*pow(d,1./3.)*distance);
quaternionProduct(*qs, *qnear, q);
}
void ompl::base::SO3StateSampler::sampleGaussian(State *state, const State * mean, const double stdDev)
{
// The standard deviation of the individual components of the tangent
// perturbation needs to be scaled so that the expected quaternion distance
// between the sampled state and the mean state is stdDev. The factor 2 is
// due to the way we define distance (see also Matt Mason's lecture notes
// on quaternions at
// http://www.cs.cmu.edu/afs/cs/academic/class/16741-s07/www/lecture7.pdf).
// The 1/sqrt(3) factor is necessary because the distribution in the tangent
// space is a 3-dimensional Gaussian, so that the *length* of a tangent
// vector needs to be scaled by 1/sqrt(3).
double rotDev = (2. * stdDev) / boost::math::constants::root_three<double>();
// CDF of N(0, 1.17) at -pi/4 is approx. .25, so there's .25 probability
// weight in each tail. Since the maximum distance in SO(3) is pi/2, we're
// essentially as likely to sample a state within distance [0, pi/4] as
// within distance [pi/4, pi/2]. With most weight in the tails (that wrap
// around in case of quaternions) we might as well sample uniformly.
if (rotDev > 1.17)
{
sampleUniform(state);
return;
}
double x = rng_.gaussian(0, rotDev), y = rng_.gaussian(0, rotDev), z = rng_.gaussian(0, rotDev),
theta = std::sqrt(x*x + y*y + z*z);
if (theta < std::numeric_limits<double>::epsilon())
space_->copyState(state, mean);
else
{
SO3StateSpace::StateType q,
*qs = static_cast<SO3StateSpace::StateType*>(state);
const SO3StateSpace::StateType *qmu = static_cast<const SO3StateSpace::StateType*>(mean);
double half_theta = theta / 2.0;
double s = sin(half_theta) / theta;
q.w = cos(half_theta);
q.x = s * x;
q.y = s * y;
q.z = s * z;
quaternionProduct(*qs, *qmu, q);
}
}
