37 #include "ompl/base/spaces/RealVectorStateProjections.h"
38 #include "ompl/util/Exception.h"
39 #include "ompl/tools/config/MagicConstants.h"
47 static inline void checkSpaceType(
const StateSpace *m)
49 if (!dynamic_cast<const RealVectorStateSpace*>(m))
50 throw Exception(
"Expected real vector state space for projection");
91 const std::vector<unsigned int> &components) :
100 const std::vector<unsigned int> &components) :
122 bounds_.resize(components_.size());
124 for (
unsigned int i = 0 ; i < components_.size() ; ++i)
126 bounds_.low[i] = bounds.
low[components_[i]];
127 bounds_.high[i] = bounds.
high[components_[i]];
134 bounds_.resize(components_.size());
135 cellSizes_.resize(components_.size());
136 for (
unsigned int i = 0 ; i < cellSizes_.size() ; ++i)
138 bounds_.low[i] = bounds.
low[components_[i]];
139 bounds_.high[i] = bounds.
high[components_[i]];
146 return projection_.mat.size1();
156 return components_.size();
161 for (
unsigned int i = 0 ; i < components_.size() ; ++i)
193 void ompl::base::RealVectorIdentityProjectionEvaluator::copyBounds(
void)
201 cellSizes_.resize(getDimension());
202 for (
unsigned int i = 0 ; i < cellSizes_.size() ; ++i)
208 copySize_ = getDimension() *
sizeof(double);
214 return space_->getDimension();
std::vector< double > low
Lower bound.
RealVectorIdentityProjectionEvaluator(const StateSpace *space, const std::vector< double > &cellSizes)
Initialize the identity projection evaluator for state space space. The indices of the kept component...
A boost shared pointer wrapper for ompl::base::StateSpace.
virtual void setup(void)
Perform configuration steps, if needed.
virtual void project(const State *state, EuclideanProjection &projection) const
Compute the projection as an array of double values.
void copyBounds()
Fill bounds_ with bounds from the state space.
virtual unsigned int getDimension(void) const
Return the dimension of the projection defined by this evaluator.
ProjectionMatrix projection_
The projection matrix.
virtual unsigned int getDimension(void) const
Return the dimension of the projection defined by this evaluator.
virtual void project(const State *state, EuclideanProjection &projection) const
Compute the projection as an array of double values.
const T * as(void) const
Cast this instance to a desired type.
const StateSpace * space_
The state space this projection operates on.
Matrix mat
Projection matrix.
std::vector< double > high
Upper bound.
virtual void defaultCellSizes(void)
Set the default cell dimensions for this projection. The default implementation of this function is e...
static const double PROJECTION_DIMENSION_SPLITS
When the cell sizes for a projection are automatically computed, this value defines the number of par...
A state space representing Rn. The distance function is the L2 norm.
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
boost::numeric::ublas::vector< double > EuclideanProjection
The datatype for state projections. This class contains a real vector.
Definition of an abstract state.
The lower and upper bounds for an Rn space.
double * values
The value of the actual vector in Rn
The definition of a state in Rn
virtual unsigned int getDimension(void) const
Return the dimension of the projection defined by this evaluator.
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 ...
RealVectorLinearProjectionEvaluator(const StateSpace *space, const std::vector< double > &cellSizes, const ProjectionMatrix::Matrix &projection)
Initialize a linear projection evaluator for state space space. The used projection matrix is project...
virtual void project(const State *state, EuclideanProjection &projection) const
Compute the projection as an array of double values.
RealVectorOrthogonalProjectionEvaluator(const StateSpace *space, const std::vector< double > &cellSizes, const std::vector< unsigned int > &components)
Initialize an orthogonal projection evaluator for state space space. The indices of the kept componen...
virtual void setup(void)
Perform configuration steps, if needed.
boost::numeric::ublas::matrix< double > Matrix
Datatype for projection matrices.
virtual void defaultCellSizes(void)
Set the default cell dimensions for this projection. The default implementation of this function is e...
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...