All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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 
45 #include <vector>
46 #include <valarray>
47 #include <iostream>
48 #include <boost/noncopyable.hpp>
49 #include <boost/numeric/ublas/matrix.hpp>
50 
51 namespace ompl
52 {
53 
54  namespace base
55  {
56 
58  typedef std::vector<int> ProjectionCoordinates;
59 
61  typedef boost::numeric::ublas::vector<double> EuclideanProjection;
62 
63 
68  {
69  public:
70 
72  typedef boost::numeric::ublas::matrix<double> Matrix;
73 
89  static Matrix ComputeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale);
90 
99  static Matrix ComputeRandom(const unsigned int from, const unsigned int to);
100 
102  void computeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale);
103 
105  void computeRandom(const unsigned int from, const unsigned int to);
106 
108  void project(const double *from, EuclideanProjection& to) const;
109 
111  void print(std::ostream &out = std::cout) const;
112 
115  };
116 
118  OMPL_CLASS_FORWARD(StateSpace);
120 
122 
123  OMPL_CLASS_FORWARD(ProjectionEvaluator);
125 
135  class ProjectionEvaluator : private boost::noncopyable
136  {
137  public:
138 
140  ProjectionEvaluator(const StateSpace *space);
141 
143  ProjectionEvaluator(const StateSpacePtr &space);
144 
145  virtual ~ProjectionEvaluator(void);
146 
148  virtual unsigned int getDimension(void) const = 0;
149 
151  virtual void project(const State *state, EuclideanProjection &projection) const = 0;
152 
160  virtual void setCellSizes(const std::vector<double> &cellSizes);
161 
164  void setCellSizes(unsigned int dim, double cellSize);
165 
169  void mulCellSizes(double factor);
170 
172  bool userConfigured(void) const;
173 
175  const std::vector<double>& getCellSizes(void) const
176  {
177  return cellSizes_;
178  }
179 
181  double getCellSizes(unsigned int dim) const;
182 
184  void checkCellSizes(void) const;
185 
191  void inferCellSizes(void);
192 
197  virtual void defaultCellSizes(void);
198 
200  virtual void setup(void);
201 
203  void computeCoordinates(const EuclideanProjection &projection, ProjectionCoordinates &coord) const;
204 
206  void computeCoordinates(const State *state, ProjectionCoordinates &coord) const
207  {
208  EuclideanProjection projection(getDimension());
209  project(state, projection);
210  computeCoordinates(projection, coord);
211  }
212 
215  {
216  return params_;
217  }
218 
220  const ParamSet& params(void) const
221  {
222  return params_;
223  }
224 
226  virtual void printSettings(std::ostream &out = std::cout) const;
227 
229  virtual void printProjection(const EuclideanProjection &projection, std::ostream &out = std::cout) const;
230 
231  protected:
232 
235 
239  std::vector<double> cellSizes_;
240 
246 
250 
253  };
254 
260  {
261  public:
262 
269  SubspaceProjectionEvaluator(const StateSpace *space, unsigned int index, const ProjectionEvaluatorPtr &projToUse = ProjectionEvaluatorPtr());
270 
271  virtual void setup(void);
272 
273  virtual unsigned int getDimension(void) const;
274 
275  virtual void project(const State *state, EuclideanProjection &projection) const;
276 
277  protected:
278 
280  unsigned int index_;
281 
287 
290  };
291 
292  }
293 
294 }
295 
296 #endif