ompl/geometric/planners/kpiece/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() : coverage(0.0), selections(1), score(1.0), iteration(0), importance(0.0)
00067                 {
00068                 }
00069 
00070                 ~CellData()
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()
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() const
00150             {
00151                 return selectBorderFraction_;
00152             }
00153 
00155             void setDimension(unsigned int dim)
00156             {
00157                 grid_.setDimension(dim);
00158             }
00159 
00161             void clear()
00162             {
00163                 freeMemory();
00164                 size_ = 0;
00165                 iteration_ = 1;
00166                 recentCell_ = NULL;
00167             }
00168 
00169             void countIteration()
00170             {
00171                 ++iteration_;
00172             }
00173 
00174             std::size_t getMotionCount() const
00175             {
00176                 return size_;
00177             }
00178 
00179             std::size_t getCellCount() const
00180             {
00181                 return grid_.size();
00182             }
00183 
00185             void freeMemory()
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() const
00285             {
00286                 return grid_;
00287             }
00288 
00289             void getPlannerData(base::PlannerData &data, int tag, bool start, const Motion *lastGoalMotion) const
00290             {
00291                 std::vector<CellData*> cdata;
00292                 grid_.getContent(cdata);
00293 
00294                 if (lastGoalMotion)
00295                     data.addGoalVertex (base::PlannerDataVertex(lastGoalMotion->state, tag));
00296 
00297                 for (unsigned int i = 0 ; i < cdata.size() ; ++i)
00298                     for (unsigned int j = 0 ; j < cdata[i]->motions.size() ; ++j)
00299                     {
00300                         if (cdata[i]->motions[j]->parent == NULL)
00301                         {
00302                             if (start)
00303                                 data.addStartVertex(base::PlannerDataVertex(cdata[i]->motions[j]->state, tag));
00304                             else
00305                                 data.addGoalVertex(base::PlannerDataVertex(cdata[i]->motions[j]->state, tag));
00306                         }
00307                         else
00308                         {
00309                             if (start)
00310                                 data.addEdge(base::PlannerDataVertex(cdata[i]->motions[j]->parent->state, tag),
00311                                              base::PlannerDataVertex(cdata[i]->motions[j]->state, tag));
00312                             else
00313                                 data.addEdge(base::PlannerDataVertex(cdata[i]->motions[j]->state, tag),
00314                                              base::PlannerDataVertex(cdata[i]->motions[j]->parent->state, tag));
00315                         }
00316                     }
00317             }
00318 
00319         private:
00320 
00322             void freeCellData(CellData *cdata)
00323             {
00324                 for (unsigned int i = 0 ; i < cdata->motions.size() ; ++i)
00325                     freeMotion_(cdata->motions[i]);
00326                 delete cdata;
00327             }
00328 
00332             static void computeImportance(Cell *cell, void*)
00333             {
00334                 CellData &cd = *(cell->data);
00335                 cd.importance =  cd.score / ((cell->neighbors + 1) * cd.coverage * cd.selections);
00336             }
00337 
00340             Grid         grid_;
00341 
00344             std::size_t  size_;
00345 
00347             unsigned int iteration_;
00348 
00350             Cell        *recentCell_;
00351 
00353             FreeMotionFn freeMotion_;
00354 
00357             double       selectBorderFraction_;
00358 
00360             RNG          rng_;
00361 
00362         };
00363     }
00364 }
00365 
00366 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines