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 
00050 namespace ompl
00051 {
00052 
00053     namespace geometric
00054     {
00055 
00057         template<typename Motion>
00058         class Discretization
00059         {
00060         public:
00061 
00063             struct CellData
00064             {
00065                 CellData(void) : coverage(0.0), selections(1), score(1.0), iteration(0), importance(0.0)
00066                 {
00067                 }
00068 
00069                 ~CellData(void)
00070                 {
00071                 }
00072 
00074                 std::vector<Motion*> motions;
00075 
00079                 double               coverage;
00080 
00083                 unsigned int         selections;
00084 
00088                 double               score;
00089 
00091                 unsigned int         iteration;
00092 
00094                 double               importance;
00095             };
00096 
00099             struct OrderCellsByImportance
00100             {
00101 
00103                 bool operator()(const CellData * const a, const CellData * const b) const
00104                 {
00105                     return a->importance > b->importance;
00106                 }
00107             };
00108 
00110             typedef GridB<CellData*, OrderCellsByImportance> Grid;
00111 
00113             typedef typename Grid::Cell  Cell;
00114 
00116             typedef typename Grid::Coord Coord;
00117 
00119             typedef typename boost::function1<void, Motion*> FreeMotionFn;
00120 
00121             Discretization(const FreeMotionFn &freeMotion) : grid_(0), size_(0), iteration_(1), recentCell_(NULL),
00122                                                              freeMotion_(freeMotion)
00123             {
00124                 grid_.onCellUpdate(computeImportance, NULL);
00125                 selectBorderFraction_ = 0.9;
00126             }
00127 
00128             ~Discretization(void)
00129             {
00130                 freeMemory();
00131             }
00132 
00139             void setBorderFraction(double bp)
00140             {
00141                 if (bp < std::numeric_limits<double>::epsilon() || bp > 1.0)
00142                     throw Exception("The fraction of time spent selecting border cells must be in the range (0,1]");
00143                 selectBorderFraction_ = bp;
00144             }
00145 
00148             double getBorderFraction(void) const
00149             {
00150                 return selectBorderFraction_;
00151             }
00152 
00154             void setDimension(unsigned int dim)
00155             {
00156                 grid_.setDimension(dim);
00157             }
00158 
00160             void clear(void)
00161             {
00162                 freeMemory();
00163                 size_ = 0;
00164                 iteration_ = 1;
00165                 recentCell_ = NULL;
00166             }
00167 
00168             void countIteration(void)
00169             {
00170                 ++iteration_;
00171             }
00172 
00173             std::size_t getMotionCount(void)
00174             {
00175                 return size_;
00176             }
00177 
00178             std::size_t getCellCount(void)
00179             {
00180                 return grid_.size();
00181             }
00182 
00184             void freeMemory(void)
00185             {
00186                 for (typename Grid::iterator it = grid_.begin(); it != grid_.end() ; ++it)
00187                     freeCellData(it->second->data);
00188                 grid_.clear();
00189             }
00190 
00199             unsigned int addMotion(Motion* motion, const Coord &coord, double dist = 0.0)
00200             {
00201                 Cell *cell = grid_.getCell(coord);
00202 
00203                 unsigned int created = 0;
00204                 if (cell)
00205                 {
00206                     cell->data->motions.push_back(motion);
00207                     cell->data->coverage += 1.0;
00208                     grid_.update(cell);
00209                 }
00210                 else
00211                 {
00212                     cell = grid_.createCell(coord);
00213                     cell->data = new CellData();
00214                     cell->data->motions.push_back(motion);
00215                     cell->data->coverage = 1.0;
00216                     cell->data->iteration = iteration_;
00217                     cell->data->selections = 1;
00218                     cell->data->score = (1.0 + log((double)(iteration_))) / (1.0 + dist);
00219                     grid_.add(cell);
00220                     recentCell_ = cell;
00221                     created = 1;
00222                 }
00223                 ++size_;
00224                 return created;
00225             }
00226 
00230             void selectMotion(Motion* &smotion, Cell* &scell)
00231             {
00232                 scell = rng_.uniform01() < std::max(selectBorderFraction_, grid_.fracExternal()) ?
00233                     grid_.topExternal() : grid_.topInternal();
00234 
00235                 // We are running on finite precision, so our update scheme will end up
00236                 // with 0 values for the score. This is where we fix the problem
00237                 if (scell->data->score < std::numeric_limits<double>::epsilon())
00238                 {
00239                     std::vector<CellData*> content;
00240                     content.reserve(grid_.size());
00241                     grid_.getContent(content);
00242                     for (typename std::vector<CellData*>::iterator it = content.begin() ; it != content.end() ; ++it)
00243                         (*it)->score += 1.0 + log((double)((*it)->iteration));
00244                     grid_.updateAll();
00245                 }
00246 
00247                 assert(scell && !scell->data->motions.empty());
00248 
00249                 ++scell->data->selections;
00250                 smotion = scell->data->motions[rng_.halfNormalInt(0, scell->data->motions.size() - 1)];
00251             }
00252 
00253             bool removeMotion(Motion *motion, const Coord &coord)
00254             {
00255                 Cell* cell = grid_.getCell(coord);
00256                 if (cell)
00257                 {
00258                     bool found = false;
00259                     for (unsigned int i = 0 ; i < cell->data->motions.size(); ++i)
00260                         if (cell->data->motions[i] == motion)
00261                         {
00262                             cell->data->motions.erase(cell->data->motions.begin() + i);
00263                             found = true;
00264                             --size_;
00265                             break;
00266                         }
00267                     if (cell->data->motions.empty())
00268                     {
00269                         grid_.remove(cell);
00270                         freeCellData(cell->data);
00271                         grid_.destroyCell(cell);
00272                     }
00273                     return found;
00274                 }
00275                 return false;
00276             }
00277 
00278             void updateCell(Cell *cell)
00279             {
00280                 grid_.update(cell);
00281             }
00282 
00283             const Grid& getGrid(void) const
00284             {
00285                 return grid_;
00286             }
00287 
00288             void getPlannerData(base::PlannerData &data, int tag) const
00289             {
00290                 std::vector<CellData*> cdata;
00291                 grid_.getContent(cdata);
00292                 for (unsigned int i = 0 ; i < cdata.size() ; ++i)
00293                     for (unsigned int j = 0 ; j < cdata[i]->motions.size() ; ++j)
00294                     {
00295                         data.recordEdge(cdata[i]->motions[j]->parent ? cdata[i]->motions[j]->parent->state : NULL, cdata[i]->motions[j]->state);
00296                         data.tagState(cdata[i]->motions[j]->state, tag);
00297                     }
00298             }
00299 
00300         private:
00301 
00303             void freeCellData(CellData *cdata)
00304             {
00305                 for (unsigned int i = 0 ; i < cdata->motions.size() ; ++i)
00306                     freeMotion_(cdata->motions[i]);
00307                 delete cdata;
00308             }
00309 
00313             static void computeImportance(Cell *cell, void*)
00314             {
00315                 CellData &cd = *(cell->data);
00316                 cd.importance =  cd.score / ((cell->neighbors + 1) * cd.coverage * cd.selections);
00317             }
00318 
00321             Grid         grid_;
00322 
00325             std::size_t  size_;
00326 
00328             unsigned int iteration_;
00329 
00331             Cell        *recentCell_;
00332 
00334             FreeMotionFn freeMotion_;
00335 
00338             double       selectBorderFraction_;
00339 
00341             RNG          rng_;
00342 
00343         };
00344     }
00345 }
00346 
00347 #endif