ProjectionEvaluator.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, Rice University
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 Rice University 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 #ifndef OMPL_BASE_PROJECTION_EVALUATOR_
38 #define OMPL_BASE_PROJECTION_EVALUATOR_
39 
40 #include "ompl/base/State.h"
41 #include "ompl/util/ClassForward.h"
42 #include "ompl/util/Console.h"
43 #include "ompl/base/GenericParam.h"
44 #include "ompl/base/spaces/RealVectorBounds.h"
45 
46 #include <vector>
47 #include <valarray>
48 #include <iostream>
49 #include <boost/noncopyable.hpp>
50 #include <boost/numeric/ublas/matrix.hpp>
51 
52 namespace ompl
53 {
54 
55  namespace base
56  {
57 
59  typedef std::vector<int> ProjectionCoordinates;
60 
62  typedef boost::numeric::ublas::vector<double> EuclideanProjection;
63 
64 
69  {
70  public:
71 
73  typedef boost::numeric::ublas::matrix<double> Matrix;
74 
91  static Matrix ComputeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale);
92 
102  static Matrix ComputeRandom(const unsigned int from, const unsigned int to);
103 
105  void computeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale);
106 
108  void computeRandom(const unsigned int from, const unsigned int to);
109 
111  void project(const double *from, EuclideanProjection& to) const;
112 
114  void print(std::ostream &out = std::cout) const;
115 
117  Matrix mat;
118  };
119 
121  OMPL_CLASS_FORWARD(StateSpace);
123 
125 
126  OMPL_CLASS_FORWARD(ProjectionEvaluator);
128 
138  class ProjectionEvaluator : private boost::noncopyable
139  {
140  public:
141 
143  ProjectionEvaluator(const StateSpace *space);
144 
146  ProjectionEvaluator(const StateSpacePtr &space);
147 
148  virtual ~ProjectionEvaluator();
149 
151  virtual unsigned int getDimension() const = 0;
152 
154  virtual void project(const State *state, EuclideanProjection &projection) const = 0;
155 
163  virtual void setCellSizes(const std::vector<double> &cellSizes);
164 
167  void setCellSizes(unsigned int dim, double cellSize);
168 
172  void mulCellSizes(double factor);
173 
175  bool userConfigured() const;
176 
178  const std::vector<double>& getCellSizes() const
179  {
180  return cellSizes_;
181  }
182 
184  double getCellSizes(unsigned int dim) const;
185 
187  void checkCellSizes() const;
188 
194  void inferCellSizes();
195 
200  virtual void defaultCellSizes();
201 
203  void checkBounds() const;
204 
206  bool hasBounds() const
207  {
208  return !bounds_.low.empty();
209  }
210 
214  void setBounds(const RealVectorBounds &bounds);
215 
218  {
219  return bounds_;
220  }
221 
223  void inferBounds();
224 
226  virtual void setup();
227 
229  void computeCoordinates(const EuclideanProjection &projection, ProjectionCoordinates &coord) const;
230 
232  void computeCoordinates(const State *state, ProjectionCoordinates &coord) const
233  {
234  EuclideanProjection projection(getDimension());
235  project(state, projection);
236  computeCoordinates(projection, coord);
237  }
238 
241  {
242  return params_;
243  }
244 
246  const ParamSet& params() const
247  {
248  return params_;
249  }
250 
252  virtual void printSettings(std::ostream &out = std::cout) const;
253 
255  virtual void printProjection(const EuclideanProjection &projection, std::ostream &out = std::cout) const;
256 
257  protected:
258 
260  void estimateBounds();
261 
264 
268  std::vector<double> cellSizes_;
269 
272 
277 
283 
287 
290  };
291 
297  {
298  public:
299 
306  SubspaceProjectionEvaluator(const StateSpace *space, unsigned int index, const ProjectionEvaluatorPtr &projToUse = ProjectionEvaluatorPtr());
307 
308  virtual void setup();
309 
310  virtual unsigned int getDimension() const;
311 
312  virtual void project(const State *state, EuclideanProjection &projection) const;
313 
314  protected:
315 
317  unsigned int index_;
318 
324 
327  };
328 
329  }
330 
331 }
332 
333 #endif
std::vector< double > cellSizes_
The size of a cell, in every dimension of the projected space, in the implicitly defined integer grid...
If the projection for a CompoundStateSpace is supposed to be the same as the one for one of its inclu...
bool cellSizesWereInferred_
Flag indicating whether projection cell sizes were automatically inferred.
A boost shared pointer wrapper for ompl::base::StateSpace.
void computeCoordinates(const State *state, ProjectionCoordinates &coord) const
Compute integer coordinates for a state.
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) ...
RealVectorBounds estimatedBounds_
An approximate bounding box for projected state values; This is the cached result of estimateBounds()...
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.
Maintain a set of parameters.
Definition: GenericParam.h:234
void computeRandom(const unsigned int from, const unsigned int to, const std::vector< double > &scale)
Wrapper for ComputeRandom(from, to, scale)
const StateSpace * space_
The state space this projection operates on.
ParamSet & params()
Get the parameters for this projection.
bool defaultCellSizes_
Flag indicating whether cell sizes have been set by the user, or whether they were inferred automatic...
Main namespace. Contains everything in this library.
Definition: Cost.h:42
unsigned int index_
The index of the subspace from which to project.
Matrix mat
Projection matrix.
const RealVectorBounds & getBounds()
Get the bounds computed/set for this projection.
const ParamSet & params() const
Get the parameters for this projection.
A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.
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
A projection matrix – it allows multiplication of real vectors by a specified matrix. The matrix can also be randomly generated.
const std::vector< double > & getCellSizes() const
Get the size (each dimension) of a grid cell.
The lower and upper bounds for an Rn space.
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.
RealVectorBounds bounds_
A bounding box for projected state values.
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...