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