Class SO3StateSampler

Inheritance Relationships

Base Type

Class Documentation

class SO3StateSampler : public ompl::base::StateSampler

State space sampler for SO(3), using quaternion representation

Public Functions

inline SO3StateSampler(const StateSpace *space)

Constructor.

virtual void sampleUniform(State *state) override

Sample a state.

virtual void sampleUniformNear(State *state, const State *near, double distance) override

To sample unit quaternions uniformly within some given distance, we sample a 3-vector from the R^3 tangent space. This vector is drawn uniformly random from a 3D ball centered at the origin with radius distance. The vector is then “wrapped” around S^3 to obtain a unit quaternion uniformly distributed around the identity quaternion within given distance. We pre-multiply this quaternion with the quaternion near to center the distribution around near.

virtual void sampleGaussian(State *state, const State *mean, double stdDev) override

Sample a state such that the expected distance between mean and state is stdDev.

To sample a unit quaternion from a Gaussian distribution, we sample a 3-vector from the R^3 tangent space using a 3D Gaussian with zero mean and covariance matrix equal to diag(stdDev^2, stdDev^2, stdDev^2). This vector is “wrapped” around S^3 to obtain a Gaussian quaternion with zero mean. We pre-multiply this quaternion with the quaternion mean to get the desired mean.