38 #define BOOST_UBLAS_SHALLOW_ARRAY_ADAPTOR
39 #include "ompl/base/StateSpace.h"
40 #include "ompl/base/ProjectionEvaluator.h"
41 #include "ompl/util/Exception.h"
42 #include "ompl/util/RandomNumbers.h"
43 #include "ompl/tools/config/MagicConstants.h"
44 #include <boost/numeric/ublas/matrix_proxy.hpp>
45 #include <boost/numeric/ublas/io.hpp>
46 #include <boost/lexical_cast.hpp>
47 #include <boost/bind.hpp>
56 using namespace boost::numeric::ublas;
59 Matrix projection(to, from);
61 for (
unsigned int j = 0 ; j < from ; ++j)
63 if (scale.size() == from && fabs(scale[j]) < std::numeric_limits<double>::epsilon())
64 boost::numeric::ublas::column(projection, j) = boost::numeric::ublas::zero_vector<double>(to);
66 for (
unsigned int i = 0 ; i < to ; ++i)
67 projection(i, j) = rng.gaussian01();
70 for (
unsigned int i = 0 ; i < to ; ++i)
72 matrix_row<Matrix> row(projection, i);
73 for (
unsigned int j = 0 ; j < i ; ++j)
75 matrix_row<Matrix> prevRow(projection, j);
77 row -= inner_prod(row, prevRow) * prevRow;
83 assert(scale.size() == from || scale.size() == 0);
84 if (scale.size() == from)
87 for (
unsigned int i = 0 ; i < from ; ++i)
89 if (fabs(scale[i]) < std::numeric_limits<double>::epsilon())
92 boost::numeric::ublas::column(projection, i) /= scale[i];
95 OMPL_WARN(
"Computed projection matrix is all 0s");
117 using namespace boost::numeric::ublas;
119 shallow_array_adaptor<const double> tmp1(
mat.size2(), from);
120 vector<double, shallow_array_adaptor<const double> > tmp2(
mat.size2(), tmp1);
121 to = prod(
mat, tmp2);
126 out <<
mat << std::endl;
131 bounds_(0), estimatedBounds_(0),
132 defaultCellSizes_(true), cellSizesWereInferred_(false)
139 bounds_(0), estimatedBounds_(0),
140 defaultCellSizes_(true), cellSizesWereInferred_(false)
145 ompl::base::ProjectionEvaluator::~ProjectionEvaluator(
void)
151 return !defaultCellSizes_ && !cellSizesWereInferred_;
156 defaultCellSizes_ =
false;
157 cellSizesWereInferred_ =
false;
158 cellSizes_ = cellSizes;
170 if (cellSizes_.size() >= dim)
171 OMPL_ERROR(
"Dimension %u is not defined for projection evaluator", dim);
174 std::vector<double> c = cellSizes_;
182 if (cellSizes_.size() > dim)
183 return cellSizes_[dim];
184 OMPL_ERROR(
"Dimension %u is not defined for projection evaluator", dim);
190 if (cellSizes_.size() == getDimension())
192 std::vector<double> c(cellSizes_.size());
193 for (std::size_t i = 0 ; i < cellSizes_.size() ; ++i)
194 c[i] = cellSizes_[i] * factor;
201 if (getDimension() <= 0)
202 throw Exception(
"Dimension of projection needs to be larger than 0");
203 if (cellSizes_.size() != getDimension())
204 throw Exception(
"Number of dimensions in projection space does not match number of cell sizes");
210 if (hasBounds() && bounds_.low.size() != getDimension())
211 throw Exception(
"Number of dimensions in projection space does not match dimension of bounds");
226 const std::size_t dim = cellSizes.size();
228 for (
unsigned int i = 0 ; i < dim ; ++i)
229 coord[i] = (
int)floor(projection(i)/cellSizes[i]);
237 if (estimatedBounds_.low.empty())
239 bounds_ = estimatedBounds_;
244 unsigned int dim = getDimension();
245 estimatedBounds_.resize(dim);
249 State *s = space_->allocState();
252 estimatedBounds_.setLow(std::numeric_limits<double>::infinity());
253 estimatedBounds_.setHigh(-std::numeric_limits<double>::infinity());
257 sampler->sampleUniform(s);
259 for (
unsigned int j = 0 ; j < dim ; ++j)
261 if (estimatedBounds_.low[j] > proj[j])
262 estimatedBounds_.low[j] = proj[j];
263 if (estimatedBounds_.high[j] < proj[j])
264 estimatedBounds_.high[j] = proj[j];
268 std::vector<double> diff(estimatedBounds_.getDifference());
269 for (
unsigned int j = 0; j < dim; ++j)
275 space_->freeState(s);
281 cellSizesWereInferred_ =
true;
284 unsigned int dim = getDimension();
285 cellSizes_.resize(dim);
286 for (
unsigned int j = 0 ; j < dim ; ++j)
289 if (cellSizes_[j] < std::numeric_limits<double>::epsilon())
292 OMPL_WARN(
"Inferred cell size for dimension %u of a projection for state space %s is 0. Setting arbitrary value of 1 instead.",
293 j, space_->getName().c_str());
300 if (defaultCellSizes_)
303 if ((cellSizes_.size() == 0 && getDimension() > 0) || cellSizesWereInferred_)
309 unsigned int dim = getDimension();
310 for (
unsigned int i = 0 ; i < dim ; ++i)
311 params_.declareParam<
double>(
"cellsize." + boost::lexical_cast<std::string>(i),
318 computeCoordinatesHelper(cellSizes_, projection, coord);
323 out <<
"Projection of dimension " << getDimension() << std::endl;
325 if (cellSizesWereInferred_)
326 out <<
" (inferred by sampling)";
329 if (defaultCellSizes_)
330 out <<
" (computed defaults)";
332 out <<
" (set by user)";
335 for (
unsigned int i = 0 ; i < cellSizes_.size() ; ++i)
337 out << cellSizes_[i];
338 if (i + 1 < cellSizes_.size())
341 out <<
']' << std::endl;
346 out << projection << std::endl;
353 throw Exception(
"Cannot construct a subspace projection evaluator for a space that is not compound");
361 proj_ = specifiedProj_;
365 throw Exception(
"No projection specified for subspace at index " + boost::lexical_cast<std::string>(index_));
367 cellSizes_ = proj_->getCellSizes();
373 return proj_->getDimension();
Definition of a compound state.
void estimateBounds(void)
Fill estimatedBounds_ with an approximate bounding box for the projection space (via sampling) ...
virtual void printSettings(std::ostream &out=std::cout) const
Print settings about this projection.
A boost shared pointer wrapper for ompl::base::StateSpace.
A boost shared pointer wrapper for ompl::base::StateSampler.
void checkBounds(void) const
Check if the projection dimension matched the dimension of the bounds.
ProjectionEvaluator(const StateSpace *space)
Construct a projection evaluator for a specific state space.
virtual void setup(void)
Perform configuration steps, if needed.
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 ...
static const double PROJECTION_EXPAND_FACTOR
When a bounding box of projected states cannot be inferred, it will be estimated by sampling states...
void print(std::ostream &out=std::cout) const
Print the contained projection matrix to a stram.
ParamSet params_
The set of parameters for this projection.
void computeRandom(const unsigned int from, const unsigned int to, const std::vector< double > &scale)
Wrapper for ComputeRandom(from, to, scale)
void inferBounds(void)
Compute an approximation of the bounds for this projection space. getBounds() will then report the co...
virtual unsigned int getDimension(void) const
Return the dimension of the projection defined by this evaluator.
const T * as(void) const
Cast this instance to a desired type.
const StateSpace * space_
The state space this projection operates on.
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...
static const unsigned int PROJECTION_EXTENTS_SAMPLES
When no cell sizes are specified for a projection, they are inferred like so:
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
bool userConfigured(void) const
Return true if any user configuration has been done to this projection evaluator (setCellSizes() was ...
A space to allow the composition of state spaces.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
unsigned int index_
The index of the subspace from which to project.
virtual void project(const State *state, EuclideanProjection &projection) const
Compute the projection as an array of double values.
Matrix mat
Projection matrix.
virtual void printProjection(const EuclideanProjection &projection, std::ostream &out=std::cout) const
Print a euclidean projection.
const std::string & getName(void) const
Get the name of the state space.
unsigned int getSubspaceCount(void) const
Get the number of state spaces that make up the compound state space.
void mulCellSizes(double factor)
Multiply the cell sizes in each dimension by a specified factor factor. This function does nothing if...
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...
std::vector< int > ProjectionCoordinates
Grid cells corresponding to a projection value are described in terms of their coordinates.
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
virtual void setup(void)
Perform configuration steps, if needed.
const std::vector< double > & getCellSizes(void) const
Get the size (each dimension) of a grid cell.
boost::numeric::ublas::vector< double > EuclideanProjection
The datatype for state projections. This class contains a real vector.
ProjectionEvaluatorPtr getDefaultProjection(void) const
Get the default projection.
Definition of an abstract state.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
void checkCellSizes(void) const
Check if cell dimensions match projection dimension.
static Matrix ComputeRandom(const unsigned int from, const unsigned int to)
Compute a random projection matrix with from columns and to rows. A vector with from elements can be ...
The exception type for ompl.
void inferCellSizes(void)
Sample the state space and decide on default cell sizes. This function is called by setup() if no cel...
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.
The lower and upper bounds for an Rn space.
State ** components
The components that make up a compound state.
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().
virtual bool isCompound(void) const
Check if the state space is compound.
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 ...
virtual void defaultCellSizes(void)
Set the default cell dimensions for this projection. The default implementation of this function is e...
void project(const double *from, EuclideanProjection &to) const
Multiply the vector from by the contained projection matrix to obtain the vector to.
boost::numeric::ublas::matrix< double > Matrix
Datatype for projection matrices.
T * as(void)
Cast this instance to a desired type.
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
void computeCoordinates(const EuclideanProjection &projection, ProjectionCoordinates &coord) const
Compute integer coordinates for a projection.