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/numeric/ublas/matrix.hpp>
50 
51 namespace ompl
52 {
53  namespace base
54  {
56  typedef std::vector<int> ProjectionCoordinates;
57 
59  typedef boost::numeric::ublas::vector<double> EuclideanProjection;
64  {
65  public:
67  typedef boost::numeric::ublas::matrix<double> Matrix;
68 
85  static Matrix ComputeRandom(const unsigned int from, const unsigned int to,
86  const std::vector<double> &scale);
87 
97  static Matrix ComputeRandom(const unsigned int from, const unsigned int to);
98 
100  void computeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale);
101 
103  void computeRandom(const unsigned int from, const unsigned int to);
104 
106  void project(const double *from, EuclideanProjection &to) const;
107 
109  void print(std::ostream &out = std::cout) const;
110 
112  Matrix mat;
113  };
114 
116  OMPL_CLASS_FORWARD(StateSpace);
118 
120 
121  OMPL_CLASS_FORWARD(ProjectionEvaluator);
123 
134  {
135  public:
136  // non-copyable
137  ProjectionEvaluator(const ProjectionEvaluator &) = delete;
138  ProjectionEvaluator &operator=(const ProjectionEvaluator &) = delete;
139 
141  ProjectionEvaluator(const StateSpace *space);
142 
144  ProjectionEvaluator(const StateSpacePtr &space);
145 
146  virtual ~ProjectionEvaluator();
147 
149  virtual unsigned int getDimension() const = 0;
150 
152  virtual void project(const State *state, EuclideanProjection &projection) const = 0;
153 
161  virtual void setCellSizes(const std::vector<double> &cellSizes);
162 
167  void setCellSizes(unsigned int dim, double cellSize);
168 
175  void mulCellSizes(double factor);
176 
179  bool userConfigured() const;
180 
182  const std::vector<double> &getCellSizes() const
183  {
184  return cellSizes_;
185  }
186 
188  double getCellSizes(unsigned int dim) const;
189 
191  void checkCellSizes() const;
192 
198  void inferCellSizes();
199 
204  virtual void defaultCellSizes();
205 
207  void checkBounds() const;
208 
210  bool hasBounds() const
211  {
212  return !bounds_.low.empty();
213  }
214 
218  void setBounds(const RealVectorBounds &bounds);
219 
222  {
223  return bounds_;
224  }
225 
228  void inferBounds();
229 
231  virtual void setup();
232 
234  void computeCoordinates(const EuclideanProjection &projection, ProjectionCoordinates &coord) const;
235 
237  void computeCoordinates(const State *state, ProjectionCoordinates &coord) const
238  {
239  EuclideanProjection projection(getDimension());
240  project(state, projection);
241  computeCoordinates(projection, coord);
242  }
243 
246  {
247  return params_;
248  }
249 
251  const ParamSet &params() const
252  {
253  return params_;
254  }
255 
257  virtual void printSettings(std::ostream &out = std::cout) const;
258 
260  virtual void printProjection(const EuclideanProjection &projection, std::ostream &out = std::cout) const;
261 
262  protected:
264  void estimateBounds();
265 
268 
272  std::vector<double> cellSizes_;
273 
276 
281 
287 
291 
294  };
295 
301  {
302  public:
309  SubspaceProjectionEvaluator(const StateSpace *space, unsigned int index,
311 
312  void setup() override;
313 
314  unsigned int getDimension() const override;
315 
316  void project(const State *state, EuclideanProjection &projection) const override;
317 
318  protected:
320  unsigned int index_;
321 
327 
331  };
332  }
333 }
334 
335 #endif
std::vector< double > cellSizes_
The size of a cell, in every dimension of the projected space, in the implicitly defined integer grid...
void print(std::ostream &out=std::cout) const
Print the contained projection matrix to a stram.
If the projection for a CompoundStateSpace is supposed to be the same as the one for one of its inclu...
const RealVectorBounds & getBounds() const
Get the bounds computed/set for this projection.
bool cellSizesWereInferred_
Flag indicating whether projection cell sizes were automatically inferred.
A shared pointer wrapper for ompl::base::StateSpace.
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()...
ParamSet params_
The set of parameters for this projection.
Maintain a set of parameters.
Definition: GenericParam.h:226
void computeRandom(const unsigned int from, const unsigned int to, const std::vector< double > &scale)
Wrapper for ComputeRandom(from, to, scale)
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.
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: AppBase.h:21
unsigned int index_
The index of the subspace from which to project.
Matrix mat
Projection matrix.
A 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:70
const ParamSet & params() const
Get the parameters for this projection.
boost::numeric::ublas::vector< double > EuclideanProjection
The datatype for state projections. This class contains a real vector.
void computeCoordinates(const State *state, ProjectionCoordinates &coord) const
Compute integer coordinates for a state.
Definition of an abstract state.
Definition: State.h:49
A projection matrix – it allows multiplication of real vectors by a specified matrix. The matrix can also be randomly generated.
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.
boost::numeric::ublas::matrix< double > Matrix
Datatype for projection matrices.
RealVectorBounds bounds_
A bounding box for projected state values.
bool hasBounds() const
Check if bounds were specified for this projection.
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...