ProjectionEvaluator.cpp
52 ompl::base::ProjectionMatrix::Matrix ompl::base::ProjectionMatrix::ComputeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale)
98 ompl::base::ProjectionMatrix::Matrix ompl::base::ProjectionMatrix::ComputeRandom(const unsigned int from, const unsigned int to)
103 void ompl::base::ProjectionMatrix::computeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale)
108 void ompl::base::ProjectionMatrix::computeRandom(const unsigned int from, const unsigned int to)
132 params_.declareParam<double>("cellsize_factor", boost::bind(&ProjectionEvaluator::mulCellSizes, this, _1));
140 params_.declareParam<double>("cellsize_factor", boost::bind(&ProjectionEvaluator::mulCellSizes, this, _1));
202 throw Exception("Number of dimensions in projection space does not match number of cell sizes");
222 static inline void computeCoordinatesHelper(const std::vector<double> &cellSizes, const EuclideanProjection &projection, ProjectionCoordinates &coord)
290 OMPL_WARN("Inferred cell size for dimension %u of a projection for state space %s is 0. Setting arbitrary value of 1 instead.",
314 void ompl::base::ProjectionEvaluator::computeCoordinates(const EuclideanProjection &projection, ProjectionCoordinates &coord) const
342 void ompl::base::ProjectionEvaluator::printProjection(const EuclideanProjection &projection, std::ostream &out) const
347 ompl::base::SubspaceProjectionEvaluator::SubspaceProjectionEvaluator(const StateSpace *space, unsigned int index, const ProjectionEvaluatorPtr &projToUse) :
351 throw Exception("Cannot construct a subspace projection evaluator for a space that is not compound");
353 throw Exception("State space " + space_->getName() + " does not have a subspace at index " + boost::lexical_cast<std::string>(index_));
363 throw Exception("No projection specified for subspace at index " + boost::lexical_cast<std::string>(index_));
374 void ompl::base::SubspaceProjectionEvaluator::project(const State *state, EuclideanProjection &projection) const
virtual StateSamplerPtr allocStateSampler() const
Allocate an instance of the state sampler for this space. This sampler will be allocated with the sam...
Definition: StateSpace.cpp:785
void checkBounds() const
Check if the projection dimension matched the dimension of the bounds.
Definition: ProjectionEvaluator.cpp:205
void resize(std::size_t size)
Change the number of dimensions for the bounds.
Definition: RealVectorBounds.cpp:83
virtual void printSettings(std::ostream &out=std::cout) const
Print settings about this projection.
Definition: ProjectionEvaluator.cpp:319
std::vector< double > cellSizes_
The size of a cell, in every dimension of the projected space, in the implicitly defined integer grid...
Definition: ProjectionEvaluator.h:268
double gaussian01()
Generate a random real using a normal distribution with mean 0 and variance 1.
Definition: RandomNumbers.h:88
void checkCellSizes() const
Check if cell dimensions match projection dimension.
Definition: ProjectionEvaluator.cpp:197
bool cellSizesWereInferred_
Flag indicating whether projection cell sizes were automatically inferred.
Definition: ProjectionEvaluator.h:286
A boost shared pointer wrapper for ompl::base::StateSpace.
A boost shared pointer wrapper for ompl::base::StateSampler.
ProjectionEvaluator(const StateSpace *space)
Construct a projection evaluator for a specific state space.
Definition: ProjectionEvaluator.cpp:127
void inferBounds()
Compute an approximation of the bounds for this projection space. getBounds() will then report the co...
Definition: ProjectionEvaluator.cpp:233
static Matrix ComputeRandom(const unsigned int from, const unsigned int to, const std::vector< double > &scale)
Compute a random projection matrix with from columns and to rows. A vector with from elements can be ...
Definition: ProjectionEvaluator.cpp:52
ProjectionEvaluatorPtr specifiedProj_
The projection that is optionally specified by the user in the constructor argument (projToUse) ...
Definition: ProjectionEvaluator.h:326
void inferCellSizes()
Sample the state space and decide on default cell sizes. This function is called by setup() if no cel...
Definition: ProjectionEvaluator.cpp:277
RealVectorBounds estimatedBounds_
An approximate bounding box for projected state values; This is the cached result of estimateBounds()...
Definition: ProjectionEvaluator.h:276
static const double PROJECTION_EXPAND_FACTOR
When a bounding box of projected states cannot be inferred, it will be estimated by sampling states...
Definition: MagicConstants.h:69
void print(std::ostream &out=std::cout) const
Print the contained projection matrix to a stram.
Definition: ProjectionEvaluator.cpp:122
void computeRandom(const unsigned int from, const unsigned int to, const std::vector< double > &scale)
Wrapper for ComputeRandom(from, to, scale)
Definition: ProjectionEvaluator.cpp:103
virtual State * allocState() const =0
Allocate a state that can store a point in the described space.
const StateSpace * space_
The state space this projection operates on.
Definition: ProjectionEvaluator.h:263
bool defaultCellSizes_
Flag indicating whether cell sizes have been set by the user, or whether they were inferred automatic...
Definition: ProjectionEvaluator.h:282
SubspaceProjectionEvaluator(const StateSpace *space, unsigned int index, const ProjectionEvaluatorPtr &projToUse=ProjectionEvaluatorPtr())
The constructor states that for space space, the projection to use is the same as the component at po...
Definition: ProjectionEvaluator.cpp:347
void setLow(double value)
Set the lower bound in each dimension to a specific value.
Definition: RealVectorBounds.cpp:42
static const unsigned int PROJECTION_EXTENTS_SAMPLES
When no cell sizes are specified for a projection, they are inferred like so:
Definition: MagicConstants.h:63
virtual void project(const State *state, EuclideanProjection &projection) const =0
Compute the projection as an array of double values.
void setHigh(double value)
Set the upper bound in each dimension to a specific value.
Definition: RealVectorBounds.cpp:47
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:54
virtual void defaultCellSizes()
Set the default cell dimensions for this projection. The default implementation of this function is e...
Definition: ProjectionEvaluator.cpp:212
A space to allow the composition of state spaces.
Definition: StateSpace.h:544
unsigned int index_
The index of the subspace from which to project.
Definition: ProjectionEvaluator.h:317
virtual void project(const State *state, EuclideanProjection &projection) const
Compute the projection as an array of double values.
Definition: ProjectionEvaluator.cpp:374
virtual void printProjection(const EuclideanProjection &projection, std::ostream &out=std::cout) const
Print a euclidean projection.
Definition: ProjectionEvaluator.cpp:342
void mulCellSizes(double factor)
Multiply the cell sizes in each dimension by a specified factor factor. This function does nothing if...
Definition: ProjectionEvaluator.cpp:186
bool userConfigured() const
Return true if any user configuration has been done to this projection evaluator (setCellSizes() was ...
Definition: ProjectionEvaluator.cpp:147
A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.
static const double PROJECTION_DIMENSION_SPLITS
When the cell sizes for a projection are automatically computed, this value defines the number of par...
Definition: MagicConstants.h:57
std::vector< int > ProjectionCoordinates
Grid cells corresponding to a projection value are described in terms of their coordinates.
Definition: ProjectionEvaluator.h:59
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
Definition: StateSpace.h:73
boost::numeric::ublas::vector< double > EuclideanProjection
The datatype for state projections. This class contains a real vector.
Definition: ProjectionEvaluator.h:62
void check() const
Check if the bounds are valid (same length for low and high, high[i] > low[i]). Throw an exception if...
Definition: RealVectorBounds.cpp:89
void declareParam(const std::string &name, const typename SpecificParam< T >::SetterFn &setter, const typename SpecificParam< T >::GetterFn &getter=typename SpecificParam< T >::GetterFn())
This function declares a parameter name, and specifies the setter and getter functions.
Definition: GenericParam.h:240
virtual void freeState(State *state) const =0
Free the memory of the allocated state.
const std::vector< double > & getCellSizes() const
Get the size (each dimension) of a grid cell.
Definition: ProjectionEvaluator.h:178
virtual unsigned int getDimension() const =0
Return the dimension of the projection defined by this evaluator.
The lower and upper bounds for an Rn space.
Definition: RealVectorBounds.h:48
void estimateBounds()
Fill estimatedBounds_ with an approximate bounding box for the projection space (via sampling) ...
Definition: ProjectionEvaluator.cpp:240
virtual bool isCompound() const
Check if the state space is compound.
Definition: StateSpace.cpp:750
void setBounds(const RealVectorBounds &bounds)
Set bounds on the projection. The PDST planner needs to known the bounds on the projection. Default bounds are automatically computed by inferCellSizes().
Definition: ProjectionEvaluator.cpp:160
unsigned int getSubspaceCount() const
Get the number of state spaces that make up the compound state space.
Definition: StateSpace.cpp:888
virtual void setCellSizes(const std::vector< double > &cellSizes)
Define the size (in each dimension) of a grid cell. The number of sizes set here must be the same as ...
Definition: ProjectionEvaluator.cpp:152
ProjectionEvaluatorPtr getDefaultProjection() const
Get the default projection.
Definition: StateSpace.cpp:709
bool hasBounds() const
Check if bounds were specified for this projection.
Definition: ProjectionEvaluator.h:206
void project(const double *from, EuclideanProjection &to) const
Multiply the vector from by the contained projection matrix to obtain the vector to.
Definition: ProjectionEvaluator.cpp:113
boost::numeric::ublas::matrix< double > Matrix
Datatype for projection matrices.
Definition: ProjectionEvaluator.h:73
std::vector< double > getDifference() const
Get the difference between the high and low bounds for each dimension: result[i] = high[i] - low[i]...
Definition: RealVectorBounds.cpp:62
virtual unsigned int getDimension() const
Return the dimension of the projection defined by this evaluator.
Definition: ProjectionEvaluator.cpp:369
RealVectorBounds bounds_
A bounding box for projected state values.
Definition: ProjectionEvaluator.h:271
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
Definition: ProjectionEvaluator.h:138
ProjectionEvaluatorPtr proj_
The projection to use. This is either the same as specifiedProj_ or, if specifiedProj_ is not initial...
Definition: ProjectionEvaluator.h:323
void computeCoordinates(const EuclideanProjection &projection, ProjectionCoordinates &coord) const
Compute integer coordinates for a projection.
Definition: ProjectionEvaluator.cpp:314