All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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 // static const double DIMENSION_UPDATE_FACTOR = 1.2;
53 
54 ompl::base::ProjectionMatrix::Matrix ompl::base::ProjectionMatrix::ComputeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale)
55 {
56  using namespace boost::numeric::ublas;
57 
58  RNG rng;
59  Matrix projection(to, from);
60 
61  for (unsigned int j = 0 ; j < from ; ++j)
62  {
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);
65  else
66  for (unsigned int i = 0 ; i < to ; ++i)
67  projection(i, j) = rng.gaussian01();
68  }
69 
70  for (unsigned int i = 0 ; i < to ; ++i)
71  {
72  matrix_row<Matrix> row(projection, i);
73  for (unsigned int j = 0 ; j < i ; ++j)
74  {
75  matrix_row<Matrix> prevRow(projection, j);
76  // subtract projection
77  row -= inner_prod(row, prevRow) * prevRow;
78  }
79  // normalize
80  row /= norm_2(row);
81  }
82 
83  assert(scale.size() == from || scale.size() == 0);
84  if (scale.size() == from)
85  {
86  unsigned int z = 0;
87  for (unsigned int i = 0 ; i < from ; ++i)
88  {
89  if (fabs(scale[i]) < std::numeric_limits<double>::epsilon())
90  z++;
91  else
92  boost::numeric::ublas::column(projection, i) /= scale[i];
93  }
94  if (z == from)
95  OMPL_WARN("Computed projection matrix is all 0s");
96  }
97  return projection;
98 }
99 
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, const std::vector<double> &scale)
106 {
107  mat = ComputeRandom(from, to, scale);
108 }
109 
110 void ompl::base::ProjectionMatrix::computeRandom(const unsigned int from, const unsigned int to)
111 {
112  mat = ComputeRandom(from, to);
113 }
114 
116 {
117  using namespace boost::numeric::ublas;
118  // create a temporary uBLAS vector from a C-style array without copying data
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);
122 }
123 
124 void ompl::base::ProjectionMatrix::print(std::ostream &out) const
125 {
126  out << mat << std::endl;
127 }
128 
130  space_(space),
131  bounds_(0), estimatedBounds_(0),
132  defaultCellSizes_(true), cellSizesWereInferred_(false)
133 {
134  params_.declareParam<double>("cellsize_factor", boost::bind(&ProjectionEvaluator::mulCellSizes, this, _1));
135 }
136 
138  space_(space.get()),
139  bounds_(0), estimatedBounds_(0),
140  defaultCellSizes_(true), cellSizesWereInferred_(false)
141 {
142  params_.declareParam<double>("cellsize_factor", boost::bind(&ProjectionEvaluator::mulCellSizes, this, _1));
143 }
144 
145 ompl::base::ProjectionEvaluator::~ProjectionEvaluator(void)
146 {
147 }
148 
150 {
151  return !defaultCellSizes_ && !cellSizesWereInferred_;
152 }
153 
154 void ompl::base::ProjectionEvaluator::setCellSizes(const std::vector<double> &cellSizes)
155 {
156  defaultCellSizes_ = false;
157  cellSizesWereInferred_ = false;
158  cellSizes_ = cellSizes;
159  checkCellSizes();
160 }
161 
163 {
164  bounds_ = bounds;
165  checkBounds();
166 }
167 
168 void ompl::base::ProjectionEvaluator::setCellSizes(unsigned int dim, double cellSize)
169 {
170  if (cellSizes_.size() >= dim)
171  OMPL_ERROR("Dimension %u is not defined for projection evaluator", dim);
172  else
173  {
174  std::vector<double> c = cellSizes_;
175  c[dim] = cellSize;
176  setCellSizes(c);
177  }
178 }
179 
180 double ompl::base::ProjectionEvaluator::getCellSizes(unsigned int dim) const
181 {
182  if (cellSizes_.size() > dim)
183  return cellSizes_[dim];
184  OMPL_ERROR("Dimension %u is not defined for projection evaluator", dim);
185  return 0.0;
186 }
187 
189 {
190  if (cellSizes_.size() == getDimension())
191  {
192  std::vector<double> c(cellSizes_.size());
193  for (std::size_t i = 0 ; i < cellSizes_.size() ; ++i)
194  c[i] = cellSizes_[i] * factor;
195  setCellSizes(c);
196  }
197 }
198 
200 {
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");
205 }
206 
208 {
209  bounds_.check();
210  if (hasBounds() && bounds_.low.size() != getDimension())
211  throw Exception("Number of dimensions in projection space does not match dimension of bounds");
212 }
213 
215 {
216 }
217 
219 namespace ompl
220 {
221  namespace base
222  {
223 
224  static inline void computeCoordinatesHelper(const std::vector<double> &cellSizes, const EuclideanProjection &projection, ProjectionCoordinates &coord)
225  {
226  const std::size_t dim = cellSizes.size();
227  coord.resize(dim);
228  for (unsigned int i = 0 ; i < dim ; ++i)
229  coord[i] = (int)floor(projection(i)/cellSizes[i]);
230  }
231  }
232 }
234 
236 {
237  if (estimatedBounds_.low.empty())
238  estimateBounds();
239  bounds_ = estimatedBounds_;
240 }
241 
243 {
244  unsigned int dim = getDimension();
245  estimatedBounds_.resize(dim);
246  if (dim > 0)
247  {
248  StateSamplerPtr sampler = space_->allocStateSampler();
249  State *s = space_->allocState();
250  EuclideanProjection proj(dim);
251 
252  estimatedBounds_.setLow(std::numeric_limits<double>::infinity());
253  estimatedBounds_.setHigh(-std::numeric_limits<double>::infinity());
254 
255  for (unsigned int i = 0 ; i < magic::PROJECTION_EXTENTS_SAMPLES ; ++i)
256  {
257  sampler->sampleUniform(s);
258  project(s, proj);
259  for (unsigned int j = 0 ; j < dim ; ++j)
260  {
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];
265  }
266  }
267  // make bounding box 10% larger (5% padding on each side)
268  std::vector<double> diff(estimatedBounds_.getDifference());
269  for (unsigned int j = 0; j < dim; ++j)
270  {
271  estimatedBounds_.low[j] -= magic::PROJECTION_EXPAND_FACTOR * diff[j];
272  estimatedBounds_.high[j] += magic::PROJECTION_EXPAND_FACTOR * diff[j];
273  }
274 
275  space_->freeState(s);
276  }
277 }
278 
280 {
281  cellSizesWereInferred_ = true;
282  if (!hasBounds())
283  inferBounds();
284  unsigned int dim = getDimension();
285  cellSizes_.resize(dim);
286  for (unsigned int j = 0 ; j < dim ; ++j)
287  {
288  cellSizes_[j] = (bounds_.high[j] - bounds_.low[j]) / magic::PROJECTION_DIMENSION_SPLITS;
289  if (cellSizes_[j] < std::numeric_limits<double>::epsilon())
290  {
291  cellSizes_[j] = 1.0;
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());
294  }
295  }
296 }
297 
299 {
300  if (defaultCellSizes_)
301  defaultCellSizes();
302 
303  if ((cellSizes_.size() == 0 && getDimension() > 0) || cellSizesWereInferred_)
304  inferCellSizes();
305 
306  checkCellSizes();
307  checkBounds();
308 
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),
312  boost::bind(&ProjectionEvaluator::setCellSizes, this, i, _1),
313  boost::bind(&ProjectionEvaluator::getCellSizes, this, i));
314 }
315 
317 {
318  computeCoordinatesHelper(cellSizes_, projection, coord);
319 }
320 
322 {
323  out << "Projection of dimension " << getDimension() << std::endl;
324  out << "Cell sizes";
325  if (cellSizesWereInferred_)
326  out << " (inferred by sampling)";
327  else
328  {
329  if (defaultCellSizes_)
330  out << " (computed defaults)";
331  else
332  out << " (set by user)";
333  }
334  out << ": [";
335  for (unsigned int i = 0 ; i < cellSizes_.size() ; ++i)
336  {
337  out << cellSizes_[i];
338  if (i + 1 < cellSizes_.size())
339  out << ' ';
340  }
341  out << ']' << std::endl;
342 }
343 
344 void ompl::base::ProjectionEvaluator::printProjection(const EuclideanProjection &projection, std::ostream &out) const
345 {
346  out << projection << std::endl;
347 }
348 
350  ProjectionEvaluator(space), index_(index), specifiedProj_(projToUse)
351 {
352  if (!space_->isCompound())
353  throw Exception("Cannot construct a subspace projection evaluator for a space that is not compound");
355  throw Exception("State space " + space_->getName() + " does not have a subspace at index " + boost::lexical_cast<std::string>(index_));
356 }
357 
359 {
360  if (specifiedProj_)
361  proj_ = specifiedProj_;
362  else
363  proj_ = space_->as<CompoundStateSpace>()->getSubspace(index_)->getDefaultProjection();
364  if (!proj_)
365  throw Exception("No projection specified for subspace at index " + boost::lexical_cast<std::string>(index_));
366 
367  cellSizes_ = proj_->getCellSizes();
369 }
370 
372 {
373  return proj_->getDimension();
374 }
375 
377 {
378  proj_->project(state->as<CompoundState>()->components[index_], projection);
379 }
Definition of a compound state.
Definition: State.h:95
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.
Definition: State.h:74
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...
Definition: RandomNumbers.h:54
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.
Definition: StateSpace.h:538
#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.
const std::string & getName(void) const
Get the name of the state space.
Definition: StateSpace.cpp:187
unsigned int getSubspaceCount(void) const
Get the number of state spaces that make up the compound state space.
Definition: StateSpace.cpp:872
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.
Definition: StateSpace.h:73
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: StateSpace.cpp:698
Definition of an abstract state.
Definition: State.h:50
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
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.
Definition: Exception.h:47
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.
Definition: GenericParam.h:222
The lower and upper bounds for an Rn space.
State ** components
The components that make up a compound state.
Definition: State.h:135
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.
Definition: StateSpace.cpp:739
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.
Definition: StateSpace.h:87
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.