ProjectionEvaluator.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 // We need this to create a temporary uBLAS vector from a C-style array without copying data
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>
48 #include <cmath>
49 #include <cstring>
50 #include <limits>
51 
52 ompl::base::ProjectionMatrix::Matrix ompl::base::ProjectionMatrix::ComputeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale)
53 {
54  using namespace boost::numeric::ublas;
55 
56  RNG rng;
57  Matrix projection(to, from);
58 
59  for (unsigned int j = 0 ; j < from ; ++j)
60  {
61  if (scale.size() == from && fabs(scale[j]) < std::numeric_limits<double>::epsilon())
62  boost::numeric::ublas::column(projection, j) = boost::numeric::ublas::zero_vector<double>(to);
63  else
64  for (unsigned int i = 0 ; i < to ; ++i)
65  projection(i, j) = rng.gaussian01();
66  }
67 
68  for (unsigned int i = 0 ; i < to ; ++i)
69  {
70  matrix_row<Matrix> row(projection, i);
71  for (unsigned int j = 0 ; j < i ; ++j)
72  {
73  matrix_row<Matrix> prevRow(projection, j);
74  // subtract projection
75  row -= inner_prod(row, prevRow) * prevRow;
76  }
77  // normalize
78  row /= norm_2(row);
79  }
80 
81  assert(scale.size() == from || scale.size() == 0);
82  if (scale.size() == from)
83  {
84  unsigned int z = 0;
85  for (unsigned int i = 0 ; i < from ; ++i)
86  {
87  if (fabs(scale[i]) < std::numeric_limits<double>::epsilon())
88  z++;
89  else
90  boost::numeric::ublas::column(projection, i) /= scale[i];
91  }
92  if (z == from)
93  OMPL_WARN("Computed projection matrix is all 0s");
94  }
95  return projection;
96 }
97 
99 {
100  return ComputeRandom(from, to, std::vector<double>());
101 }
102 
103 void ompl::base::ProjectionMatrix::computeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale)
104 {
105  mat = ComputeRandom(from, to, scale);
106 }
107 
108 void ompl::base::ProjectionMatrix::computeRandom(const unsigned int from, const unsigned int to)
109 {
110  mat = ComputeRandom(from, to);
111 }
112 
114 {
115  using namespace boost::numeric::ublas;
116  // create a temporary uBLAS vector from a C-style array without copying data
117  shallow_array_adaptor<const double> tmp1(mat.size2(), from);
118  vector<double, shallow_array_adaptor<const double> > tmp2(mat.size2(), tmp1);
119  to = prod(mat, tmp2);
120 }
121 
122 void ompl::base::ProjectionMatrix::print(std::ostream &out) const
123 {
124  out << mat << std::endl;
125 }
126 
128  space_(space),
129  bounds_(0), estimatedBounds_(0),
130  defaultCellSizes_(true), cellSizesWereInferred_(false)
131 {
132  params_.declareParam<double>("cellsize_factor", boost::bind(&ProjectionEvaluator::mulCellSizes, this, _1));
133 }
134 
136  space_(space.get()),
137  bounds_(0), estimatedBounds_(0),
139 {
140  params_.declareParam<double>("cellsize_factor", boost::bind(&ProjectionEvaluator::mulCellSizes, this, _1));
141 }
142 
143 ompl::base::ProjectionEvaluator::~ProjectionEvaluator()
144 {
145 }
146 
148 {
150 }
151 
152 void ompl::base::ProjectionEvaluator::setCellSizes(const std::vector<double> &cellSizes)
153 {
154  defaultCellSizes_ = false;
155  cellSizesWereInferred_ = false;
156  cellSizes_ = cellSizes;
157  checkCellSizes();
158 }
159 
161 {
162  bounds_ = bounds;
163  checkBounds();
164 }
165 
166 void ompl::base::ProjectionEvaluator::setCellSizes(unsigned int dim, double cellSize)
167 {
168  if (cellSizes_.size() >= dim)
169  OMPL_ERROR("Dimension %u is not defined for projection evaluator", dim);
170  else
171  {
172  std::vector<double> c = cellSizes_;
173  c[dim] = cellSize;
174  setCellSizes(c);
175  }
176 }
177 
178 double ompl::base::ProjectionEvaluator::getCellSizes(unsigned int dim) const
179 {
180  if (cellSizes_.size() > dim)
181  return cellSizes_[dim];
182  OMPL_ERROR("Dimension %u is not defined for projection evaluator", dim);
183  return 0.0;
184 }
185 
187 {
188  if (cellSizes_.size() == getDimension())
189  {
190  std::vector<double> c(cellSizes_.size());
191  for (std::size_t i = 0 ; i < cellSizes_.size() ; ++i)
192  c[i] = cellSizes_[i] * factor;
193  setCellSizes(c);
194  }
195 }
196 
198 {
199  if (getDimension() <= 0)
200  throw Exception("Dimension of projection needs to be larger than 0");
201  if (cellSizes_.size() != getDimension())
202  throw Exception("Number of dimensions in projection space does not match number of cell sizes");
203 }
204 
206 {
207  bounds_.check();
208  if (hasBounds() && bounds_.low.size() != getDimension())
209  throw Exception("Number of dimensions in projection space does not match dimension of bounds");
210 }
211 
213 {
214 }
215 
217 namespace ompl
218 {
219  namespace base
220  {
221 
222  static inline void computeCoordinatesHelper(const std::vector<double> &cellSizes, const EuclideanProjection &projection, ProjectionCoordinates &coord)
223  {
224  const std::size_t dim = cellSizes.size();
225  coord.resize(dim);
226  for (unsigned int i = 0 ; i < dim ; ++i)
227  coord[i] = (int)floor(projection(i)/cellSizes[i]);
228  }
229  }
230 }
232 
234 {
235  if (estimatedBounds_.low.empty())
236  estimateBounds();
238 }
239 
241 {
242  unsigned int dim = getDimension();
244  if (dim > 0)
245  {
247  State *s = space_->allocState();
248  EuclideanProjection proj(dim);
249 
250  estimatedBounds_.setLow(std::numeric_limits<double>::infinity());
251  estimatedBounds_.setHigh(-std::numeric_limits<double>::infinity());
252 
253  for (unsigned int i = 0 ; i < magic::PROJECTION_EXTENTS_SAMPLES ; ++i)
254  {
255  sampler->sampleUniform(s);
256  project(s, proj);
257  for (unsigned int j = 0 ; j < dim ; ++j)
258  {
259  if (estimatedBounds_.low[j] > proj[j])
260  estimatedBounds_.low[j] = proj[j];
261  if (estimatedBounds_.high[j] < proj[j])
262  estimatedBounds_.high[j] = proj[j];
263  }
264  }
265  // make bounding box 10% larger (5% padding on each side)
266  std::vector<double> diff(estimatedBounds_.getDifference());
267  for (unsigned int j = 0; j < dim; ++j)
268  {
271  }
272 
273  space_->freeState(s);
274  }
275 }
276 
278 {
279  cellSizesWereInferred_ = true;
280  if (!hasBounds())
281  inferBounds();
282  unsigned int dim = getDimension();
283  cellSizes_.resize(dim);
284  for (unsigned int j = 0 ; j < dim ; ++j)
285  {
287  if (cellSizes_[j] < std::numeric_limits<double>::epsilon())
288  {
289  cellSizes_[j] = 1.0;
290  OMPL_WARN("Inferred cell size for dimension %u of a projection for state space %s is 0. Setting arbitrary value of 1 instead.",
291  j, space_->getName().c_str());
292  }
293  }
294 }
295 
297 {
298  if (defaultCellSizes_)
300 
301  if ((cellSizes_.size() == 0 && getDimension() > 0) || cellSizesWereInferred_)
302  inferCellSizes();
303 
304  checkCellSizes();
305  checkBounds();
306 
307  unsigned int dim = getDimension();
308  for (unsigned int i = 0 ; i < dim ; ++i)
309  params_.declareParam<double>("cellsize." + boost::lexical_cast<std::string>(i),
310  boost::bind(&ProjectionEvaluator::setCellSizes, this, i, _1),
311  boost::bind(&ProjectionEvaluator::getCellSizes, this, i));
312 }
313 
315 {
316  computeCoordinatesHelper(cellSizes_, projection, coord);
317 }
318 
320 {
321  out << "Projection of dimension " << getDimension() << std::endl;
322  out << "Cell sizes";
324  out << " (inferred by sampling)";
325  else
326  {
327  if (defaultCellSizes_)
328  out << " (computed defaults)";
329  else
330  out << " (set by user)";
331  }
332  out << ": [";
333  for (unsigned int i = 0 ; i < cellSizes_.size() ; ++i)
334  {
335  out << cellSizes_[i];
336  if (i + 1 < cellSizes_.size())
337  out << ' ';
338  }
339  out << ']' << std::endl;
340 }
341 
342 void ompl::base::ProjectionEvaluator::printProjection(const EuclideanProjection &projection, std::ostream &out) const
343 {
344  out << projection << std::endl;
345 }
346 
348  ProjectionEvaluator(space), index_(index), specifiedProj_(projToUse)
349 {
350  if (!space_->isCompound())
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_));
354 }
355 
357 {
358  if (specifiedProj_)
360  else
362  if (!proj_)
363  throw Exception("No projection specified for subspace at index " + boost::lexical_cast<std::string>(index_));
364 
365  cellSizes_ = proj_->getCellSizes();
367 }
368 
370 {
371  return proj_->getDimension();
372 }
373 
375 {
376  proj_->project(state->as<CompoundState>()->components[index_], projection);
377 }
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 of a compound state.
Definition: State.h:95
void resize(std::size_t size)
Change the number of dimensions for the bounds.
virtual void printSettings(std::ostream &out=std::cout) const
Print settings about this projection.
std::vector< double > cellSizes_
The size of a cell, in every dimension of the projected space, in the implicitly defined integer grid...
double gaussian01()
Generate a random real using a normal distribution with mean 0 and variance 1.
Definition: RandomNumbers.h:88
virtual void setup()
Perform configuration steps, if needed.
std::vector< double > low
Lower bound.
void checkCellSizes() const
Check if cell dimensions match projection dimension.
bool cellSizesWereInferred_
Flag indicating whether projection cell sizes were automatically inferred.
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.
void inferBounds()
Compute an approximation of the bounds for this projection space. getBounds() will then report the co...
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 ...
ProjectionEvaluatorPtr specifiedProj_
The projection that is optionally specified by the user in the constructor argument (projToUse) ...
void inferCellSizes()
Sample the state space and decide on default cell sizes. This function is called by setup() if no cel...
RealVectorBounds estimatedBounds_
An approximate bounding box for projected state values; This is the cached result of estimateBounds()...
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)
T * as()
Cast this instance to a desired type.
Definition: StateSpace.h:87
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.
bool defaultCellSizes_
Flag indicating whether cell sizes have been set by the user, or whether they were inferred automatic...
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...
void setLow(double value)
Set the lower bound in each dimension to a specific value.
static const unsigned int PROJECTION_EXTENTS_SAMPLES
When no cell sizes are specified for a projection, they are inferred like so:
Main namespace. Contains everything in this library.
Definition: Cost.h:42
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.
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...
A space to allow the composition of state spaces.
Definition: StateSpace.h:544
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
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.
std::vector< double > high
Upper bound.
void mulCellSizes(double factor)
Multiply the cell sizes in each dimension by a specified factor factor. This function does nothing if...
bool userConfigured() const
Return true if any user configuration has been done to this projection evaluator (setCellSizes() was ...
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.
Definition: StateSpace.h:73
boost::numeric::ublas::vector< double > EuclideanProjection
The datatype for state projections. This class contains a real vector.
Definition of an abstract state.
Definition: State.h:50
void check() const
Check if the bounds are valid (same length for low and high, high[i] > low[i]). Throw an exception if...
The exception type for ompl.
Definition: Exception.h:47
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.
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.
void estimateBounds()
Fill estimatedBounds_ with an approximate bounding box for the projection space (via sampling) ...
State ** components
The components that make up a compound state.
Definition: State.h:142
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().
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 ...
const std::string & getName() const
Get the name of the state space.
Definition: StateSpace.cpp:198
ProjectionEvaluatorPtr getDefaultProjection() const
Get the default projection.
Definition: StateSpace.cpp:709
const T * as() const
Cast this instance to a desired type.
Definition: State.h:74
bool hasBounds() const
Check if bounds were specified for this projection.
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.
std::vector< double > getDifference() const
Get the difference between the high and low bounds for each dimension: result[i] = high[i] - low[i]...
virtual unsigned int getDimension() const
Return the dimension of the projection defined by this evaluator.
RealVectorBounds bounds_
A bounding box for projected state values.
virtual void setup()
Perform configuration steps, if needed.
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
ProjectionEvaluatorPtr proj_
The projection to use. This is either the same as specifiedProj_ or, if specifiedProj_ is not initial...
void computeCoordinates(const EuclideanProjection &projection, ProjectionCoordinates &coord) const
Compute integer coordinates for a projection.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66