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 <vector>
00044 #include <valarray>
00045 #include <iostream>
00046 #include <boost/noncopyable.hpp>
00047 #include <boost/numeric/ublas/matrix.hpp>
00048
00049 namespace ompl
00050 {
00051
00052 namespace base
00053 {
00054
00056 typedef std::vector<int> ProjectionCoordinates;
00057
00059 typedef boost::numeric::ublas::vector<double> EuclideanProjection;
00060
00061
00065 class ProjectionMatrix
00066 {
00067 public:
00068
00070 typedef boost::numeric::ublas::matrix<double> Matrix;
00071
00087 static Matrix ComputeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale);
00088
00097 static Matrix ComputeRandom(const unsigned int from, const unsigned int to);
00098
00100 void computeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale);
00101
00103 void computeRandom(const unsigned int from, const unsigned int to);
00104
00106 void project(const double *from, EuclideanProjection& to) const;
00107
00109 void print(std::ostream &out = std::cout) const;
00110
00112 Matrix mat;
00113 };
00114
00115 ClassForward(StateSpace);
00116
00118 ClassForward(ProjectionEvaluator);
00119
00129 class ProjectionEvaluator : private boost::noncopyable
00130 {
00131 public:
00132
00134 ProjectionEvaluator(const StateSpace *space) : space_(space), defaultCellSizes_(true), cellSizesWereInferred_(false)
00135 {
00136 }
00137
00139 ProjectionEvaluator(const StateSpacePtr &space) : space_(space.get()), defaultCellSizes_(true), cellSizesWereInferred_(false)
00140 {
00141 }
00142
00143 virtual ~ProjectionEvaluator(void)
00144 {
00145 }
00146
00148 virtual unsigned int getDimension(void) const = 0;
00149
00151 virtual void project(const State *state, EuclideanProjection &projection) const = 0;
00152
00160 void setCellSizes(const std::vector<double> &cellSizes);
00161
00163 bool userConfigured(void) const;
00164
00166 const std::vector<double>& getCellSizes(void) const
00167 {
00168 return cellSizes_;
00169 }
00170
00172 void checkCellSizes(void) const;
00173
00179 void inferCellSizes(void);
00180
00185 virtual void defaultCellSizes(void);
00186
00188 virtual void setup(void);
00189
00191 void computeCoordinates(const EuclideanProjection &projection, ProjectionCoordinates &coord) const;
00192
00194 void computeCoordinates(const State *state, ProjectionCoordinates &coord) const
00195 {
00196 EuclideanProjection projection(getDimension());
00197 project(state, projection);
00198 computeCoordinates(projection, coord);
00199 }
00200
00202 virtual void printSettings(std::ostream &out = std::cout) const;
00203
00205 virtual void printProjection(const EuclideanProjection &projection, std::ostream &out = std::cout) const;
00206
00207 protected:
00208
00210 const StateSpace *space_;
00211
00215 std::vector<double> cellSizes_;
00216
00221 bool defaultCellSizes_;
00222
00225 bool cellSizesWereInferred_;
00226
00228 msg::Interface msg_;
00229 };
00230
00231 }
00232
00233 }
00234
00235 #endif