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 <cmath>
47 #include <cstring>
48 #include <limits>
49 #include <utility>
50 
52  const unsigned int to,
53  const std::vector<double> &scale)
54 {
55  namespace nu = boost::numeric::ublas;
56 
57  RNG rng;
58  Matrix projection(to, from);
59 
60  for (unsigned int j = 0; j < from; ++j)
61  {
62  if (scale.size() == from && fabs(scale[j]) < std::numeric_limits<double>::epsilon())
63  nu::column(projection, j) = nu::zero_vector<double>(to);
64  else
65  for (unsigned int i = 0; i < to; ++i)
66  projection(i, j) = rng.gaussian01();
67  }
68 
69  for (unsigned int i = 0; i < to; ++i)
70  {
71  nu::matrix_row<Matrix> row(projection, i);
72  for (unsigned int j = 0; j < i; ++j)
73  {
74  nu::matrix_row<Matrix> prevRow(projection, j);
75  // subtract projection
76  row -= inner_prod(row, prevRow) * prevRow;
77  }
78  // normalize
79  row /= norm_2(row);
80  }
81 
82  assert(scale.size() == from || scale.size() == 0);
83  if (scale.size() == from)
84  {
85  unsigned int z = 0;
86  for (unsigned int i = 0; i < from; ++i)
87  {
88  if (fabs(scale[i]) < std::numeric_limits<double>::epsilon())
89  z++;
90  else
91  nu::column(projection, i) /= scale[i];
92  }
93  if (z == from)
94  OMPL_WARN("Computed projection matrix is all 0s");
95  }
96  return projection;
97 }
98 
100  const unsigned int to)
101 {
102  return ComputeRandom(from, to, std::vector<double>());
103 }
104 
105 void ompl::base::ProjectionMatrix::computeRandom(const unsigned int from, const unsigned int to,
106  const std::vector<double> &scale)
107 {
108  mat = ComputeRandom(from, to, scale);
109 }
110 
111 void ompl::base::ProjectionMatrix::computeRandom(const unsigned int from, const unsigned int to)
112 {
113  mat = ComputeRandom(from, to);
114 }
115 
117 {
118  namespace nu = boost::numeric::ublas;
119  // create a temporary uBLAS vector from a C-style array without copying data
120  nu::shallow_array_adaptor<const double> tmp1(mat.size2(), from);
121  nu::vector<double, nu::shallow_array_adaptor<const double>> tmp2(mat.size2(), tmp1);
122  to = prod(mat, tmp2);
123 }
124 
125 void ompl::base::ProjectionMatrix::print(std::ostream &out) const
126 {
127  out << mat << std::endl;
128 }
129 
130 ompl::base::ProjectionEvaluator::ProjectionEvaluator(const StateSpace *space)
131  : space_(space), bounds_(0), estimatedBounds_(0), defaultCellSizes_(true), cellSizesWereInferred_(false)
132 {
133  params_.declareParam<double>("cellsize_factor", [this](double factor)
134  {
135  mulCellSizes(factor);
136  });
137 }
138 
139 ompl::base::ProjectionEvaluator::ProjectionEvaluator(const StateSpacePtr &space)
140  : space_(space.get()), bounds_(0), estimatedBounds_(0), defaultCellSizes_(true), cellSizesWereInferred_(false)
141 {
142  params_.declareParam<double>("cellsize_factor", [this](double factor)
143  {
144  mulCellSizes(factor);
145  });
146 }
147 
148 ompl::base::ProjectionEvaluator::~ProjectionEvaluator() = default;
149 
151 {
153 }
154 
155 void ompl::base::ProjectionEvaluator::setCellSizes(const std::vector<double> &cellSizes)
156 {
157  defaultCellSizes_ = false;
158  cellSizesWereInferred_ = false;
159  cellSizes_ = cellSizes;
160  checkCellSizes();
161 }
162 
164 {
165  bounds_ = bounds;
166  checkBounds();
167 }
168 
169 void ompl::base::ProjectionEvaluator::setCellSizes(unsigned int dim, double cellSize)
170 {
171  if (cellSizes_.size() >= dim)
172  OMPL_ERROR("Dimension %u is not defined for projection evaluator", dim);
173  else
174  {
175  std::vector<double> c = cellSizes_;
176  c[dim] = cellSize;
177  setCellSizes(c);
178  }
179 }
180 
181 double ompl::base::ProjectionEvaluator::getCellSizes(unsigned int dim) const
182 {
183  if (cellSizes_.size() > dim)
184  return cellSizes_[dim];
185  OMPL_ERROR("Dimension %u is not defined for projection evaluator", dim);
186  return 0.0;
187 }
188 
190 {
191  if (cellSizes_.size() == getDimension())
192  {
193  std::vector<double> c(cellSizes_.size());
194  for (std::size_t i = 0; i < cellSizes_.size(); ++i)
195  c[i] = cellSizes_[i] * factor;
196  setCellSizes(c);
197  }
198 }
199 
201 {
202  if (getDimension() <= 0)
203  throw Exception("Dimension of projection needs to be larger than 0");
204  if (cellSizes_.size() != getDimension())
205  throw Exception("Number of dimensions in projection space does not match number of cell sizes");
206 }
207 
209 {
210  bounds_.check();
211  if (hasBounds() && bounds_.low.size() != getDimension())
212  throw Exception("Number of dimensions in projection space does not match dimension of bounds");
213 }
214 
216 {
217 }
218 
220 namespace ompl
221 {
222  namespace base
223  {
224  static inline void computeCoordinatesHelper(const std::vector<double> &cellSizes,
225  const EuclideanProjection &projection, ProjectionCoordinates &coord)
226  {
227  const std::size_t dim = cellSizes.size();
228  coord.resize(dim);
229  for (unsigned int i = 0; i < dim; ++i)
230  coord[i] = (int)floor(projection(i) / cellSizes[i]);
231  }
232  }
233 }
235 
237 {
238  if (estimatedBounds_.low.empty())
239  estimateBounds();
241 }
242 
244 {
245  unsigned int dim = getDimension();
247  if (dim > 0)
248  {
250  State *s = space_->allocState();
251  EuclideanProjection proj(dim);
252 
253  estimatedBounds_.setLow(std::numeric_limits<double>::infinity());
254  estimatedBounds_.setHigh(-std::numeric_limits<double>::infinity());
255 
256  for (unsigned int i = 0; i < magic::PROJECTION_EXTENTS_SAMPLES; ++i)
257  {
258  sampler->sampleUniform(s);
259  project(s, proj);
260  for (unsigned int j = 0; j < dim; ++j)
261  {
262  if (estimatedBounds_.low[j] > proj[j])
263  estimatedBounds_.low[j] = proj[j];
264  if (estimatedBounds_.high[j] < proj[j])
265  estimatedBounds_.high[j] = proj[j];
266  }
267  }
268  // make bounding box 10% larger (5% padding on each side)
269  std::vector<double> diff(estimatedBounds_.getDifference());
270  for (unsigned int j = 0; j < dim; ++j)
271  {
274  }
275 
276  space_->freeState(s);
277  }
278 }
279 
281 {
282  cellSizesWereInferred_ = true;
283  if (!hasBounds())
284  inferBounds();
285  unsigned int dim = getDimension();
286  cellSizes_.resize(dim);
287  for (unsigned int j = 0; j < dim; ++j)
288  {
290  if (cellSizes_[j] < std::numeric_limits<double>::epsilon())
291  {
292  cellSizes_[j] = 1.0;
293  OMPL_WARN("Inferred cell size for dimension %u of a projection for state space %s is 0. Setting arbitrary "
294  "value of 1 instead.",
295  j, space_->getName().c_str());
296  }
297  }
298 }
299 
301 {
302  if (defaultCellSizes_)
304 
305  if ((cellSizes_.size() == 0 && getDimension() > 0) || cellSizesWereInferred_)
306  inferCellSizes();
307 
308  checkCellSizes();
309  checkBounds();
310 
311  unsigned int dim = getDimension();
312  for (unsigned int i = 0; i < dim; ++i)
313  params_.declareParam<double>("cellsize." + std::to_string(i),
314  [this, i](double cellsize)
315  {
316  setCellSizes(i, cellsize);
317  },
318  [this, i]
319  {
320  return getCellSizes(i);
321  });
322 }
323 
325  ProjectionCoordinates &coord) const
326 {
327  computeCoordinatesHelper(cellSizes_, projection, coord);
328 }
329 
331 {
332  out << "Projection of dimension " << getDimension() << std::endl;
333  out << "Cell sizes";
335  out << " (inferred by sampling)";
336  else
337  {
338  if (defaultCellSizes_)
339  out << " (computed defaults)";
340  else
341  out << " (set by user)";
342  }
343  out << ": [";
344  for (unsigned int i = 0; i < cellSizes_.size(); ++i)
345  {
346  out << cellSizes_[i];
347  if (i + 1 < cellSizes_.size())
348  out << ' ';
349  }
350  out << ']' << std::endl;
351 }
352 
353 void ompl::base::ProjectionEvaluator::printProjection(const EuclideanProjection &projection, std::ostream &out) const
354 {
355  out << projection << std::endl;
356 }
357 
359  ProjectionEvaluatorPtr projToUse)
360  : ProjectionEvaluator(space), index_(index), specifiedProj_(std::move(projToUse))
361 {
362  if (!space_->isCompound())
363  throw Exception("Cannot construct a subspace projection evaluator for a space that is not compound");
365  throw Exception("State space " + space_->getName() + " does not have a subspace at index " +
366  std::to_string(index_));
367 }
368 
370 {
371  if (specifiedProj_)
373  else
375  if (!proj_)
376  throw Exception("No projection specified for subspace at index " + std::to_string(index_));
377 
378  cellSizes_ = proj_->getCellSizes();
380 }
381 
383 {
384  return proj_->getDimension();
385 }
386 
388 {
389  proj_->project(state->as<CompoundState>()->components[index_], projection);
390 }
unsigned int getSubspaceCount() const
Get the number of state spaces that make up the compound state space.
Definition of a compound state.
Definition: State.h:86
void resize(std::size_t size)
Change the number of dimensions for the bounds.
ProjectionEvaluatorPtr getDefaultProjection() const
Get the default projection.
virtual unsigned int getDimension() const =0
Return the dimension of the projection defined by this evaluator.
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:94
void print(std::ostream &out=std::cout) const
Print the contained projection matrix to a stram.
std::vector< double > low
Lower bound.
bool cellSizesWereInferred_
Flag indicating whether projection cell sizes were automatically inferred.
void computeRandom(unsigned int from, unsigned int to, const std::vector< double > &scale)
Wrapper for ComputeRandom(from, to, scale)
A shared pointer wrapper for ompl::base::StateSpace.
A shared pointer wrapper for ompl::base::StateSampler.
void checkBounds() const
Check if the projection dimension matched the dimension of the bounds.
void inferBounds()
Compute an approximation of the bounds for this projection space. getBounds() will then report the co...
STL namespace.
SubspaceProjectionEvaluator(const StateSpace *space, unsigned int index, ProjectionEvaluatorPtr projToUse=ProjectionEvaluatorPtr())
The constructor states that for space space, the projection to use is the same as the component at po...
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...
ParamSet params_
The set of parameters for this projection.
const std::string & getName() const
Get the name of the state space.
Definition: StateSpace.cpp:207
T * as()
Cast this instance to a desired type.
Definition: StateSpace.h:87
unsigned int getDimension() const override
Return the dimension of the projection defined by this evaluator.
const std::vector< double > & getCellSizes() const
Get the size (each dimension) of a grid cell.
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...
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: AppBase.h:21
void setHigh(double value)
Set the upper bound in each dimension to a specific value.
std::vector< double > getDifference() const
Get the difference between the high and low bounds for each dimension: result[i] = high[i] - low[i]...
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:58
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
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:573
#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.
Matrix mat
Projection matrix.
virtual bool isCompound() const
Check if the state space is compound.
virtual StateSamplerPtr allocStateSampler() const
Allocate an instance of the state sampler for this space. This sampler will be allocated with the sam...
std::vector< double > high
Upper bound.
void check() const
Check if the bounds are valid (same length for low and high, high[i] > low[i]). Throw an exception if...
void mulCellSizes(double factor)
Multiply the cell sizes in each dimension by a specified factor factor. This function does nothing if...
A 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:70
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:49
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
void project(const State *state, EuclideanProjection &projection) const override
Compute the projection as an array of double values.
virtual void project(const State *state, EuclideanProjection &projection) const =0
Compute the projection as an array of double values.
The exception type for ompl.
Definition: Exception.h:46
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:232
virtual void printProjection(const EuclideanProjection &projection, std::ostream &out=std::cout) const
Print a euclidean projection.
void project(const double *from, EuclideanProjection &to) const
Multiply the vector from by the contained projection matrix to obtain the vector to.
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:128
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 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 ...
void computeCoordinates(const EuclideanProjection &projection, ProjectionCoordinates &coord) const
Compute integer coordinates for a projection.
virtual void freeState(State *state) const =0
Free the memory of the allocated state.
virtual void printSettings(std::ostream &out=std::cout) const
Print settings about this projection.
virtual State * allocState() const =0
Allocate a state that can store a point in the described space.
boost::numeric::ublas::matrix< double > Matrix
Datatype for projection matrices.
bool userConfigured() const
Return true if any user configuration has been done to this projection evaluator (setCellSizes() was ...
RealVectorBounds bounds_
A bounding box for projected state values.
void checkCellSizes() const
Check if cell dimensions match projection dimension.
bool hasBounds() const
Check if bounds were specified for this projection.
void setup() override
Perform configuration steps, if needed.
virtual void setup()
Perform configuration steps, if needed.
static Matrix ComputeRandom(unsigned int from, 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 ...
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...