All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
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 <vector>
00044 #include <valarray>
00045 #include <iostream>
00046 #include <boost/noncopyable.hpp>
00047 
00048 namespace ompl
00049 {
00050 
00051     namespace base
00052     {
00053 
00055         typedef std::vector<int> ProjectionCoordinates;
00056 
00058         class EuclideanProjection
00059         {
00060         public:
00061 
00063             EuclideanProjection(unsigned int n) : values(new double[n])
00064             {
00065             }
00066 
00067             ~EuclideanProjection(void)
00068             {
00069                 delete[] values;
00070             }
00071 
00073             double operator[](unsigned int i) const
00074             {
00075                 return values[i];
00076             }
00077 
00079             double& operator[](unsigned int i)
00080             {
00081                 return values[i];
00082             }
00083 
00085             double *values;
00086         };
00087 
00091         class ProjectionMatrix
00092         {
00093         public:
00094 
00096             typedef std::vector< std::valarray<double> > Matrix;
00097 
00113             static Matrix ComputeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale);
00114 
00123             static Matrix ComputeRandom(const unsigned int from, const unsigned int to);
00124 
00126             void computeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale);
00127 
00129             void computeRandom(const unsigned int from, const unsigned int to);
00130 
00132             void project(const double *from, double *to) const;
00133 
00135             void print(std::ostream &out = std::cout) const;
00136 
00138             Matrix mat;
00139         };
00140 
00141         ClassForward(StateManifold);
00142 
00144         ClassForward(ProjectionEvaluator);
00145 
00155         class ProjectionEvaluator : private boost::noncopyable
00156         {
00157         public:
00158 
00160             ProjectionEvaluator(const StateManifold *manifold) : manifold_(manifold), defaultCellSizes_(true), cellSizesWereInferred_(false)
00161             {
00162             }
00163 
00165             ProjectionEvaluator(const StateManifoldPtr &manifold) : manifold_(manifold.get()), defaultCellSizes_(true), cellSizesWereInferred_(false)
00166             {
00167             }
00168 
00169             virtual ~ProjectionEvaluator(void)
00170             {
00171             }
00172 
00174             virtual unsigned int getDimension(void) const = 0;
00175 
00177             virtual void project(const State *state, EuclideanProjection &projection) const = 0;
00178 
00186             void setCellSizes(const std::vector<double> &cellSizes);
00187 
00189             bool userConfigured(void) const;
00190 
00192             const std::vector<double>& getCellSizes(void) const
00193             {
00194                 return cellSizes_;
00195             }
00196 
00198             void checkCellSizes(void) const;
00199 
00205             void inferCellSizes(void);
00206 
00211             void computeCellSizes(const std::vector<const State*> &states);
00212 
00217             virtual void defaultCellSizes(void);
00218 
00220             virtual void setup(void);
00221 
00223             void computeCoordinates(const EuclideanProjection &projection, ProjectionCoordinates &coord) const;
00224 
00226             void computeCoordinates(const State *state, ProjectionCoordinates &coord) const
00227             {
00228                 EuclideanProjection projection(getDimension());
00229                 project(state, projection);
00230                 computeCoordinates(projection, coord);
00231             }
00232 
00234             virtual void printSettings(std::ostream &out = std::cout) const;
00235 
00237             virtual void printProjection(const EuclideanProjection &projection, std::ostream &out = std::cout) const;
00238 
00239         protected:
00240 
00242             const StateManifold *manifold_;
00243 
00247             std::vector<double>  cellSizes_;
00248 
00253             bool                 defaultCellSizes_;
00254 
00257             bool                 cellSizesWereInferred_;
00258 
00260             msg::Interface       msg_;
00261         };
00262 
00263     }
00264 
00265 }
00266 
00267 #endif
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator