All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
Discretization.h
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, 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_GEOMETRIC_PLANNERS_KPIECE_DISCRETIZATION_
00038 #define OMPL_GEOMETRIC_PLANNERS_KPIECE_DISCRETIZATION_
00039 
00040 #include "ompl/base/Planner.h"
00041 #include "ompl/datastructures/GridB.h"
00042 #include "ompl/util/Exception.h"
00043 #include <boost/function.hpp>
00044 #include <vector>
00045 #include <limits>
00046 #include <cassert>
00047 #include <utility>
00048 #include <cstdlib>
00049 #include <cmath>
00050 
00051 namespace ompl
00052 {
00053 
00054     namespace geometric
00055     {
00056 
00058         template<typename Motion>
00059         class Discretization
00060         {
00061         public:
00062 
00064             struct CellData
00065             {
00066                 CellData(void) : coverage(0.0), selections(1), score(1.0), iteration(0), importance(0.0)
00067                 {
00068                 }
00069 
00070                 ~CellData(void)
00071                 {
00072                 }
00073 
00075                 std::vector<Motion*> motions;
00076 
00080                 double               coverage;
00081 
00084                 unsigned int         selections;
00085 
00089                 double               score;
00090 
00092                 unsigned int         iteration;
00093 
00095                 double               importance;
00096             };
00097 
00100             struct OrderCellsByImportance
00101             {
00102 
00104                 bool operator()(const CellData * const a, const CellData * const b) const
00105                 {
00106                     return a->importance > b->importance;
00107                 }
00108             };
00109 
00111             typedef GridB<CellData*, OrderCellsByImportance> Grid;
00112 
00114             typedef typename Grid::Cell  Cell;
00115 
00117             typedef typename Grid::Coord Coord;
00118 
00120             typedef typename boost::function<void(Motion*)> FreeMotionFn;
00121 
00122             Discretization(const FreeMotionFn &freeMotion) : grid_(0), size_(0), iteration_(1), recentCell_(NULL),
00123                                                              freeMotion_(freeMotion)
00124             {
00125                 grid_.onCellUpdate(computeImportance, NULL);
00126                 selectBorderFraction_ = 0.9;
00127             }
00128 
00129             ~Discretization(void)
00130             {
00131                 freeMemory();
00132             }
00133 
00140             void setBorderFraction(double bp)
00141             {
00142                 if (bp < std::numeric_limits<double>::epsilon() || bp > 1.0)
00143                     throw Exception("The fraction of time spent selecting border cells must be in the range (0,1]");
00144                 selectBorderFraction_ = bp;
00145             }
00146 
00149             double getBorderFraction(void) const
00150             {
00151                 return selectBorderFraction_;
00152             }
00153 
00155             void setDimension(unsigned int dim)
00156             {
00157                 grid_.setDimension(dim);
00158             }
00159 
00161             void clear(void)
00162             {
00163                 freeMemory();
00164                 size_ = 0;
00165                 iteration_ = 1;
00166                 recentCell_ = NULL;
00167             }
00168 
00169             void countIteration(void)
00170             {
00171                 ++iteration_;
00172             }
00173 
00174             std::size_t getMotionCount(void) const
00175             {
00176                 return size_;
00177             }
00178 
00179             std::size_t getCellCount(void) const
00180             {
00181                 return grid_.size();
00182             }
00183 
00185             void freeMemory(void)
00186             {
00187                 for (typename Grid::iterator it = grid_.begin(); it != grid_.end() ; ++it)
00188                     freeCellData(it->second->data);
00189                 grid_.clear();
00190             }
00191 
00200             unsigned int addMotion(Motion* motion, const Coord &coord, double dist = 0.0)
00201             {
00202                 Cell *cell = grid_.getCell(coord);
00203 
00204                 unsigned int created = 0;
00205                 if (cell)
00206                 {
00207                     cell->data->motions.push_back(motion);
00208                     cell->data->coverage += 1.0;
00209                     grid_.update(cell);
00210                 }
00211                 else
00212                 {
00213                     cell = grid_.createCell(coord);
00214                     cell->data = new CellData();
00215                     cell->data->motions.push_back(motion);
00216                     cell->data->coverage = 1.0;
00217                     cell->data->iteration = iteration_;
00218                     cell->data->selections = 1;
00219                     cell->data->score = (1.0 + log((double)(iteration_))) / (1.0 + dist);
00220                     grid_.add(cell);
00221                     recentCell_ = cell;
00222                     created = 1;
00223                 }
00224                 ++size_;
00225                 return created;
00226             }
00227 
00231             void selectMotion(Motion* &smotion, Cell* &scell)
00232             {
00233                 scell = rng_.uniform01() < std::max(selectBorderFraction_, grid_.fracExternal()) ?
00234                     grid_.topExternal() : grid_.topInternal();
00235 
00236                 // We are running on finite precision, so our update scheme will end up
00237                 // with 0 values for the score. This is where we fix the problem
00238                 if (scell->data->score < std::numeric_limits<double>::epsilon())
00239                 {
00240                     std::vector<CellData*> content;
00241                     content.reserve(grid_.size());
00242                     grid_.getContent(content);
00243                     for (typename std::vector<CellData*>::iterator it = content.begin() ; it != content.end() ; ++it)
00244                         (*it)->score += 1.0 + log((double)((*it)->iteration));
00245                     grid_.updateAll();
00246                 }
00247 
00248                 assert(scell && !scell->data->motions.empty());
00249 
00250                 ++scell->data->selections;
00251                 smotion = scell->data->motions[rng_.halfNormalInt(0, scell->data->motions.size() - 1)];
00252             }
00253 
00254             bool removeMotion(Motion *motion, const Coord &coord)
00255             {
00256                 Cell* cell = grid_.getCell(coord);
00257                 if (cell)
00258                 {
00259                     bool found = false;
00260                     for (unsigned int i = 0 ; i < cell->data->motions.size(); ++i)
00261                         if (cell->data->motions[i] == motion)
00262                         {
00263                             cell->data->motions.erase(cell->data->motions.begin() + i);
00264                             found = true;
00265                             --size_;
00266                             break;
00267                         }
00268                     if (cell->data->motions.empty())
00269                     {
00270                         grid_.remove(cell);
00271                         freeCellData(cell->data);
00272                         grid_.destroyCell(cell);
00273                     }
00274                     return found;
00275                 }
00276                 return false;
00277             }
00278 
00279             void updateCell(Cell *cell)
00280             {
00281                 grid_.update(cell);
00282             }
00283 
00284             const Grid& getGrid(void) const
00285             {
00286                 return grid_;
00287             }
00288 
00289             void getPlannerData(base::PlannerData &data, int tag) const
00290             {
00291                 std::vector<CellData*> cdata;
00292                 grid_.getContent(cdata);
00293                 for (unsigned int i = 0 ; i < cdata.size() ; ++i)
00294                     for (unsigned int j = 0 ; j < cdata[i]->motions.size() ; ++j)
00295                     {
00296                         data.recordEdge(cdata[i]->motions[j]->parent ? cdata[i]->motions[j]->parent->state : NULL, cdata[i]->motions[j]->state);
00297                         data.tagState(cdata[i]->motions[j]->state, tag);
00298                     }
00299             }
00300 
00301         private:
00302 
00304             void freeCellData(CellData *cdata)
00305             {
00306                 for (unsigned int i = 0 ; i < cdata->motions.size() ; ++i)
00307                     freeMotion_(cdata->motions[i]);
00308                 delete cdata;
00309             }
00310 
00314             static void computeImportance(Cell *cell, void*)
00315             {
00316                 CellData &cd = *(cell->data);
00317                 cd.importance =  cd.score / ((cell->neighbors + 1) * cd.coverage * cd.selections);
00318             }
00319 
00322             Grid         grid_;
00323 
00326             std::size_t  size_;
00327 
00329             unsigned int iteration_;
00330 
00332             Cell        *recentCell_;
00333 
00335             FreeMotionFn freeMotion_;
00336 
00339             double       selectBorderFraction_;
00340 
00342             RNG          rng_;
00343 
00344         };
00345     }
00346 }
00347 
00348 #endif
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends