00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef OMPL_BASE_PROJECTION_EVALUATOR_
00038 #define OMPL_BASE_PROJECTION_EVALUATOR_
00039
00040 #include "ompl/base/State.h"
00041 #include "ompl/util/ClassForward.h"
00042 #include "ompl/util/Console.h"
00043 #include "ompl/base/GenericParam.h"
00044
00045 #include <vector>
00046 #include <valarray>
00047 #include <iostream>
00048 #include <boost/noncopyable.hpp>
00049 #include <boost/numeric/ublas/matrix.hpp>
00050
00051 namespace ompl
00052 {
00053
00054 namespace base
00055 {
00056
00058 typedef std::vector<int> ProjectionCoordinates;
00059
00061 typedef boost::numeric::ublas::vector<double> EuclideanProjection;
00062
00063
00067 class ProjectionMatrix
00068 {
00069 public:
00070
00072 typedef boost::numeric::ublas::matrix<double> Matrix;
00073
00089 static Matrix ComputeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale);
00090
00099 static Matrix ComputeRandom(const unsigned int from, const unsigned int to);
00100
00102 void computeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale);
00103
00105 void computeRandom(const unsigned int from, const unsigned int to);
00106
00108 void project(const double *from, EuclideanProjection& to) const;
00109
00111 void print(std::ostream &out = std::cout) const;
00112
00114 Matrix mat;
00115 };
00116
00118 ClassForward(StateSpace);
00120
00122
00123 ClassForward(ProjectionEvaluator);
00125
00135 class ProjectionEvaluator : private boost::noncopyable
00136 {
00137 public:
00138
00140 ProjectionEvaluator(const StateSpace *space);
00141
00143 ProjectionEvaluator(const StateSpacePtr &space);
00144
00145 virtual ~ProjectionEvaluator(void);
00146
00148 virtual unsigned int getDimension(void) const = 0;
00149
00151 virtual void project(const State *state, EuclideanProjection &projection) const = 0;
00152
00160 virtual void setCellSizes(const std::vector<double> &cellSizes);
00161
00164 void setCellSizes(unsigned int dim, double cellSize);
00165
00169 void mulCellSizes(double factor);
00170
00172 bool userConfigured(void) const;
00173
00175 const std::vector<double>& getCellSizes(void) const
00176 {
00177 return cellSizes_;
00178 }
00179
00181 double getCellSizes(unsigned int dim) const;
00182
00184 void checkCellSizes(void) const;
00185
00191 void inferCellSizes(void);
00192
00197 virtual void defaultCellSizes(void);
00198
00200 virtual void setup(void);
00201
00203 void computeCoordinates(const EuclideanProjection &projection, ProjectionCoordinates &coord) const;
00204
00206 void computeCoordinates(const State *state, ProjectionCoordinates &coord) const
00207 {
00208 EuclideanProjection projection(getDimension());
00209 project(state, projection);
00210 computeCoordinates(projection, coord);
00211 }
00212
00214 ParamSet& params(void)
00215 {
00216 return params_;
00217 }
00218
00220 const ParamSet& params(void) const
00221 {
00222 return params_;
00223 }
00224
00226 virtual void printSettings(std::ostream &out = std::cout) const;
00227
00229 virtual void printProjection(const EuclideanProjection &projection, std::ostream &out = std::cout) const;
00230
00231 protected:
00232
00234 const StateSpace *space_;
00235
00239 std::vector<double> cellSizes_;
00240
00245 bool defaultCellSizes_;
00246
00249 bool cellSizesWereInferred_;
00250
00252 ParamSet params_;
00253 };
00254
00259 class SubspaceProjectionEvaluator : public ProjectionEvaluator
00260 {
00261 public:
00262
00269 SubspaceProjectionEvaluator(const StateSpace *space, unsigned int index, const ProjectionEvaluatorPtr &projToUse = ProjectionEvaluatorPtr());
00270
00271 virtual void setup(void);
00272
00273 virtual unsigned int getDimension(void) const;
00274
00275 virtual void project(const State *state, EuclideanProjection &projection) const;
00276
00277 protected:
00278
00280 unsigned int index_;
00281
00286 ProjectionEvaluatorPtr proj_;
00287
00289 ProjectionEvaluatorPtr specifiedProj_;
00290 };
00291
00292 }
00293
00294 }
00295
00296 #endif