State space sampler for SO(3), using quaternion representation. More...
#include <SO3StateSpace.h>
Public Member Functions | |
SO3StateSampler (const StateSpace *space) | |
Constructor. | |
virtual void | sampleUniform (State *state) |
Sample a state. | |
virtual void | sampleUniformNear (State *state, const State *near, const double distance) |
To sample uniformly within some given distance, we first compute a rotation q about a uniformly random unit vector by an angle uniformly distributed between 0 and 2*distance, with a correction to account for the larger volume of states at increasing distance. (Quaternion distance is equal to half the angle in axis-angle representation.) The resulting state is then the quaternion product of near and q. | |
virtual void | sampleGaussian (State *state, const State *mean, const double stdDev) |
To sample sample a unit quaternion from a Gaussian, distribution we first compute a rotation q about a uniformly random unit vector by an angle sampled from a Gaussian with mean equal to 0 and standard deviation equal to 2*stdDev, and with a correction to account for the larger volume of states at increasing distance. (Quaternion distance is equal to half the angle in axis-angle representation.) The resulting state is then the quaternion product of near and q. |
State space sampler for SO(3), using quaternion representation.
Definition at line 48 of file SO3StateSpace.h.