ompl/base/ProjectionEvaluator.h
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Rice University 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Rice University nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Ioan Sucan */ 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 #include "ompl/base/spaces/RealVectorBounds.h" 00045 00046 #include <vector> 00047 #include <valarray> 00048 #include <iostream> 00049 #include <boost/noncopyable.hpp> 00050 #include <boost/numeric/ublas/matrix.hpp> 00051 00052 namespace ompl 00053 { 00054 00055 namespace base 00056 { 00057 00059 typedef std::vector<int> ProjectionCoordinates; 00060 00062 typedef boost::numeric::ublas::vector<double> EuclideanProjection; 00063 00064 00068 class ProjectionMatrix 00069 { 00070 public: 00071 00073 typedef boost::numeric::ublas::matrix<double> Matrix; 00074 00091 static Matrix ComputeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale); 00092 00102 static Matrix ComputeRandom(const unsigned int from, const unsigned int to); 00103 00105 void computeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale); 00106 00108 void computeRandom(const unsigned int from, const unsigned int to); 00109 00111 void project(const double *from, EuclideanProjection& to) const; 00112 00114 void print(std::ostream &out = std::cout) const; 00115 00117 Matrix mat; 00118 }; 00119 00121 OMPL_CLASS_FORWARD(StateSpace); 00123 00125 00126 OMPL_CLASS_FORWARD(ProjectionEvaluator); 00128 00138 class ProjectionEvaluator : private boost::noncopyable 00139 { 00140 public: 00141 00143 ProjectionEvaluator(const StateSpace *space); 00144 00146 ProjectionEvaluator(const StateSpacePtr &space); 00147 00148 virtual ~ProjectionEvaluator(); 00149 00151 virtual unsigned int getDimension() const = 0; 00152 00154 virtual void project(const State *state, EuclideanProjection &projection) const = 0; 00155 00163 virtual void setCellSizes(const std::vector<double> &cellSizes); 00164 00167 void setCellSizes(unsigned int dim, double cellSize); 00168 00172 void mulCellSizes(double factor); 00173 00175 bool userConfigured() const; 00176 00178 const std::vector<double>& getCellSizes() const 00179 { 00180 return cellSizes_; 00181 } 00182 00184 double getCellSizes(unsigned int dim) const; 00185 00187 void checkCellSizes() const; 00188 00194 void inferCellSizes(); 00195 00200 virtual void defaultCellSizes(); 00201 00203 void checkBounds() const; 00204 00206 bool hasBounds() const 00207 { 00208 return !bounds_.low.empty(); 00209 } 00210 00214 void setBounds(const RealVectorBounds &bounds); 00215 00217 const RealVectorBounds& getBounds() 00218 { 00219 return bounds_; 00220 } 00221 00223 void inferBounds(); 00224 00226 virtual void setup(); 00227 00229 void computeCoordinates(const EuclideanProjection &projection, ProjectionCoordinates &coord) const; 00230 00232 void computeCoordinates(const State *state, ProjectionCoordinates &coord) const 00233 { 00234 EuclideanProjection projection(getDimension()); 00235 project(state, projection); 00236 computeCoordinates(projection, coord); 00237 } 00238 00240 ParamSet& params() 00241 { 00242 return params_; 00243 } 00244 00246 const ParamSet& params() const 00247 { 00248 return params_; 00249 } 00250 00252 virtual void printSettings(std::ostream &out = std::cout) const; 00253 00255 virtual void printProjection(const EuclideanProjection &projection, std::ostream &out = std::cout) const; 00256 00257 protected: 00258 00260 void estimateBounds(); 00261 00263 const StateSpace *space_; 00264 00268 std::vector<double> cellSizes_; 00269 00271 RealVectorBounds bounds_; 00272 00276 RealVectorBounds estimatedBounds_; 00277 00282 bool defaultCellSizes_; 00283 00286 bool cellSizesWereInferred_; 00287 00289 ParamSet params_; 00290 }; 00291 00296 class SubspaceProjectionEvaluator : public ProjectionEvaluator 00297 { 00298 public: 00299 00306 SubspaceProjectionEvaluator(const StateSpace *space, unsigned int index, const ProjectionEvaluatorPtr &projToUse = ProjectionEvaluatorPtr()); 00307 00308 virtual void setup(); 00309 00310 virtual unsigned int getDimension() const; 00311 00312 virtual void project(const State *state, EuclideanProjection &projection) const; 00313 00314 protected: 00315 00317 unsigned int index_; 00318 00323 ProjectionEvaluatorPtr proj_; 00324 00326 ProjectionEvaluatorPtr specifiedProj_; 00327 }; 00328 00329 } 00330 00331 } 00332 00333 #endif