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); } }