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 #include "ompl/base/StateSpace.h"
38 #include "ompl/base/ProjectionEvaluator.h"
39 #include "ompl/util/Exception.h"
40 #include "ompl/util/RandomNumbers.h"
41 #include "ompl/tools/config/MagicConstants.h"
42 #include <Eigen/SVD>
43 #include <cmath>
44 #include <cstring>
45 #include <limits>
46 #include <utility>
47 
49  const unsigned int to,
50  const std::vector<double> &scale)
51 {
52  RNG rng;
53  Matrix projection(to, from);
54 
55  for (unsigned int j = 0; j < from; ++j)
56  {
57  if (scale.size() == from && fabs(scale[j]) < std::numeric_limits<double>::epsilon())
58  projection.col(j).setZero();
59  else
60  for (unsigned int i = 0; i < to; ++i)
61  projection(i, j) = rng.gaussian01();
62  }
63 
64  projection = Eigen::JacobiSVD<Eigen::MatrixXd>(projection, Eigen::ComputeThinV).matrixV().transpose();
65 
66  assert(scale.size() == from || scale.size() == 0);
67  if (scale.size() == from)
68  {
69  unsigned int z = 0;
70  for (unsigned int i = 0; i < from; ++i)
71  {
72  if (fabs(scale[i]) < std::numeric_limits<double>::epsilon())
73  z++;
74  else
75  projection.col(i) /= scale[i];
76  }
77  if (z == from)
78  OMPL_WARN("Computed projection matrix is all 0s");
79  }
80  return projection;
81 }
82 
84  const unsigned int to)
85 {
86  return ComputeRandom(from, to, std::vector<double>());
87 }
88 
89 void ompl::base::ProjectionMatrix::computeRandom(const unsigned int from, const unsigned int to,
90  const std::vector<double> &scale)
91 {
92  mat = ComputeRandom(from, to, scale);
93 }
94 
95 void ompl::base::ProjectionMatrix::computeRandom(const unsigned int from, const unsigned int to)
96 {
97  mat = ComputeRandom(from, to);
98 }
99 
100 void ompl::base::ProjectionMatrix::project(const double *from, Eigen::Ref<Eigen::VectorXd> to) const
101 {
102  to = mat * Eigen::Map<const Eigen::VectorXd>(from, mat.cols());
103 }
104 
105 void ompl::base::ProjectionMatrix::print(std::ostream &out) const
106 {
107  out << mat << std::endl;
108 }
109 
110 ompl::base::ProjectionEvaluator::ProjectionEvaluator(const StateSpace *space)
111  : space_(space), bounds_(0), estimatedBounds_(0), defaultCellSizes_(true), cellSizesWereInferred_(false)
112 {
113  params_.declareParam<double>("cellsize_factor", [this](double factor) { mulCellSizes(factor); });
114 }
115 
116 ompl::base::ProjectionEvaluator::ProjectionEvaluator(const StateSpacePtr &space)
117  : space_(space.get()), bounds_(0), estimatedBounds_(0), defaultCellSizes_(true), cellSizesWereInferred_(false)
118 {
119  params_.declareParam<double>("cellsize_factor", [this](double factor) { mulCellSizes(factor); });
120 }
121 
122 ompl::base::ProjectionEvaluator::~ProjectionEvaluator() = default;
123 
125 {
126  return !defaultCellSizes_ && !cellSizesWereInferred_;
127 }
128 
129 void ompl::base::ProjectionEvaluator::setCellSizes(const std::vector<double> &cellSizes)
130 {
131  defaultCellSizes_ = false;
132  cellSizesWereInferred_ = false;
133  cellSizes_ = cellSizes;
134  checkCellSizes();
135 }
136 
138 {
139  bounds_ = bounds;
140  checkBounds();
141 }
142 
143 void ompl::base::ProjectionEvaluator::setCellSizes(unsigned int dim, double cellSize)
144 {
145  if (cellSizes_.size() >= dim)
146  OMPL_ERROR("Dimension %u is not defined for projection evaluator", dim);
147  else
148  {
149  std::vector<double> c = cellSizes_;
150  c[dim] = cellSize;
151  setCellSizes(c);
152  }
153 }
154 
155 double ompl::base::ProjectionEvaluator::getCellSizes(unsigned int dim) const
156 {
157  if (cellSizes_.size() > dim)
158  return cellSizes_[dim];
159  OMPL_ERROR("Dimension %u is not defined for projection evaluator", dim);
160  return 0.0;
161 }
162 
164 {
165  if (cellSizes_.size() == getDimension())
166  {
167  std::vector<double> c(cellSizes_.size());
168  for (std::size_t i = 0; i < cellSizes_.size(); ++i)
169  c[i] = cellSizes_[i] * factor;
170  setCellSizes(c);
171  }
172 }
173 
175 {
176  if (getDimension() <= 0)
177  throw Exception("Dimension of projection needs to be larger than 0");
178  if (cellSizes_.size() != getDimension())
179  throw Exception("Number of dimensions in projection space does not match number of cell sizes");
180 }
181 
183 {
184  bounds_.check();
185  if (hasBounds() && bounds_.low.size() != getDimension())
186  throw Exception("Number of dimensions in projection space does not match dimension of bounds");
187 }
188 
190 {
191 }
192 
194 namespace ompl
195 {
196  namespace base
197  {
198  static inline void computeCoordinatesHelper(const std::vector<double> &cellSizes,
199  const Eigen::Ref<Eigen::VectorXd> &projection,
200  Eigen::Ref<Eigen::VectorXi> coord)
201  {
202  // compute floor(projection ./ cellSizes)
203  coord = projection.cwiseQuotient(Eigen::Map<const Eigen::VectorXd>(cellSizes.data(), cellSizes.size()))
204  .unaryExpr((double (*)(double))std::floor)
205  .cast<int>();
206  }
207  } // namespace base
208 } // namespace ompl
210 
212 {
213  if (estimatedBounds_.low.empty())
214  estimateBounds();
215  bounds_ = estimatedBounds_;
216 }
217 
219 {
220  unsigned int dim = getDimension();
221  estimatedBounds_.resize(dim);
222  if (dim > 0)
223  {
224  StateSamplerPtr sampler = space_->allocStateSampler();
225  State *s = space_->allocState();
226  Eigen::VectorXd proj(dim);
227 
228  estimatedBounds_.setLow(std::numeric_limits<double>::infinity());
229  estimatedBounds_.setHigh(-std::numeric_limits<double>::infinity());
230 
231  for (unsigned int i = 0; i < magic::PROJECTION_EXTENTS_SAMPLES; ++i)
232  {
233  sampler->sampleUniform(s);
234  project(s, proj);
235  for (unsigned int j = 0; j < dim; ++j)
236  {
237  if (estimatedBounds_.low[j] > proj[j])
238  estimatedBounds_.low[j] = proj[j];
239  if (estimatedBounds_.high[j] < proj[j])
240  estimatedBounds_.high[j] = proj[j];
241  }
242  }
243  // make bounding box 10% larger (5% padding on each side)
244  std::vector<double> diff(estimatedBounds_.getDifference());
245  for (unsigned int j = 0; j < dim; ++j)
246  {
247  estimatedBounds_.low[j] -= magic::PROJECTION_EXPAND_FACTOR * diff[j];
248  estimatedBounds_.high[j] += magic::PROJECTION_EXPAND_FACTOR * diff[j];
249  }
250 
251  space_->freeState(s);
252  }
253 }
254 
256 {
257  cellSizesWereInferred_ = true;
258  if (!hasBounds())
259  inferBounds();
260  unsigned int dim = getDimension();
261  cellSizes_.resize(dim);
262  for (unsigned int j = 0; j < dim; ++j)
263  {
264  cellSizes_[j] = (bounds_.high[j] - bounds_.low[j]) / magic::PROJECTION_DIMENSION_SPLITS;
265  if (cellSizes_[j] < std::numeric_limits<double>::epsilon())
266  {
267  cellSizes_[j] = 1.0;
268  OMPL_WARN("Inferred cell size for dimension %u of a projection for state space %s is 0. Setting arbitrary "
269  "value of 1 instead.",
270  j, space_->getName().c_str());
271  }
272  }
273 }
274 
276 {
277  if (defaultCellSizes_)
278  defaultCellSizes();
279 
280  if ((cellSizes_.size() == 0 && getDimension() > 0) || cellSizesWereInferred_)
281  inferCellSizes();
282 
283  checkCellSizes();
284  checkBounds();
285 
286  unsigned int dim = getDimension();
287  for (unsigned int i = 0; i < dim; ++i)
288  params_.declareParam<double>("cellsize." + std::to_string(i),
289  [this, i](double cellsize) { setCellSizes(i, cellsize); },
290  [this, i] { return getCellSizes(i); });
291 }
292 
293 void ompl::base::ProjectionEvaluator::computeCoordinates(const Eigen::Ref<Eigen::VectorXd> &projection,
294  Eigen::Ref<Eigen::VectorXi> coord) const
295 {
296  computeCoordinatesHelper(cellSizes_, projection, coord);
297 }
298 
300 {
301  out << "Projection of dimension " << getDimension() << std::endl;
302  out << "Cell sizes";
303  if (cellSizesWereInferred_)
304  out << " (inferred by sampling)";
305  else
306  {
307  if (defaultCellSizes_)
308  out << " (computed defaults)";
309  else
310  out << " (set by user)";
311  }
312  out << ": [";
313  for (unsigned int i = 0; i < cellSizes_.size(); ++i)
314  {
315  out << cellSizes_[i];
316  if (i + 1 < cellSizes_.size())
317  out << ' ';
318  }
319  out << ']' << std::endl;
320 }
321 
322 void ompl::base::ProjectionEvaluator::printProjection(const Eigen::Ref<Eigen::VectorXd> &projection,
323  std::ostream &out) const
324 {
325  out << projection << std::endl;
326 }
327 
329  ProjectionEvaluatorPtr projToUse)
330  : ProjectionEvaluator(space), index_(index), specifiedProj_(std::move(projToUse))
331 {
332  if (!space_->isCompound())
333  throw Exception("Cannot construct a subspace projection evaluator for a space that is not compound");
335  throw Exception("State space " + space_->getName() + " does not have a subspace at index " +
336  std::to_string(index_));
337 }
338 
340 {
341  if (specifiedProj_)
342  proj_ = specifiedProj_;
343  else
344  proj_ = space_->as<CompoundStateSpace>()->getSubspace(index_)->getDefaultProjection();
345  if (!proj_)
346  throw Exception("No projection specified for subspace at index " + std::to_string(index_));
347 
348  cellSizes_ = proj_->getCellSizes();
350 }
351 
353 {
354  return proj_->getDimension();
355 }
356 
357 void ompl::base::SubspaceProjectionEvaluator::project(const State *state, Eigen::Ref<Eigen::VectorXd> projection) const
358 {
359  proj_->project(state->as<CompoundState>()->components[index_], projection);
360 }
void computeCoordinates(const Eigen::Ref< Eigen::VectorXd > &projection, Eigen::Ref< Eigen::VectorXi > coord) const
Compute integer coordinates for a projection.
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
ProjectionEvaluatorPtr getDefaultProjection() const
Get the default projection.
double gaussian01()
Generate a random real using a normal distribution with mean 0 and variance 1.
Definition: RandomNumbers.h:92
void print(std::ostream &out=std::cout) const
Print the contained projection matrix to a stram.
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 project(const double *from, Eigen::Ref< Eigen::VectorXd > to) const
Multiply the vector from by the contained projection matrix to obtain the vector to.
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...
void inferCellSizes()
Sample the state space and decide on default cell sizes. This function is called by setup() if no cel...
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:195
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.
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
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:56
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
Eigen::MatrixXd Matrix
Datatype for projection matrices.
unsigned int index_
The index of the subspace from which to project.
virtual bool isCompound() const
Check if the state space is compound.
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...
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
Definition: StateSpace.h:70
Definition of an abstract state.
Definition: State.h:49
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
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
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 ...
virtual void printSettings(std::ostream &out=std::cout) const
Print settings about this projection.
virtual void printProjection(const Eigen::Ref< Eigen::VectorXd > &projection, std::ostream &out=std::cout) const
Print a euclidean projection.
bool userConfigured() const
Return true if any user configuration has been done to this projection evaluator (setCellSizes() was ...
void project(const State *state, Eigen::Ref< Eigen::VectorXd > projection) const override
Compute the projection as an array of double values.
void checkCellSizes() const
Check if cell dimensions match projection dimension.
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...