All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
Discretization.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #ifndef OMPL_GEOMETRIC_PLANNERS_KPIECE_DISCRETIZATION_
38 #define OMPL_GEOMETRIC_PLANNERS_KPIECE_DISCRETIZATION_
39 
40 #include "ompl/base/Planner.h"
41 #include "ompl/datastructures/GridB.h"
42 #include "ompl/util/Exception.h"
43 #include <boost/function.hpp>
44 #include <vector>
45 #include <limits>
46 #include <cassert>
47 #include <utility>
48 #include <cstdlib>
49 #include <cmath>
50 
51 namespace ompl
52 {
53 
54  namespace geometric
55  {
56 
58  template<typename Motion>
60  {
61  public:
62 
64  struct CellData
65  {
66  CellData(void) : coverage(0.0), selections(1), score(1.0), iteration(0), importance(0.0)
67  {
68  }
69 
70  ~CellData(void)
71  {
72  }
73 
75  std::vector<Motion*> motions;
76 
80  double coverage;
81 
84  unsigned int selections;
85 
89  double score;
90 
92  unsigned int iteration;
93 
95  double importance;
96  };
97 
101  {
102 
104  bool operator()(const CellData * const a, const CellData * const b) const
105  {
106  return a->importance > b->importance;
107  }
108  };
109 
112 
114  typedef typename Grid::Cell Cell;
115 
117  typedef typename Grid::Coord Coord;
118 
120  typedef typename boost::function<void(Motion*)> FreeMotionFn;
121 
122  Discretization(const FreeMotionFn &freeMotion) : grid_(0), size_(0), iteration_(1), recentCell_(NULL),
123  freeMotion_(freeMotion)
124  {
125  grid_.onCellUpdate(computeImportance, NULL);
126  selectBorderFraction_ = 0.9;
127  }
128 
129  ~Discretization(void)
130  {
131  freeMemory();
132  }
133 
140  void setBorderFraction(double bp)
141  {
142  if (bp < std::numeric_limits<double>::epsilon() || bp > 1.0)
143  throw Exception("The fraction of time spent selecting border cells must be in the range (0,1]");
144  selectBorderFraction_ = bp;
145  }
146 
149  double getBorderFraction(void) const
150  {
151  return selectBorderFraction_;
152  }
153 
155  void setDimension(unsigned int dim)
156  {
157  grid_.setDimension(dim);
158  }
159 
161  void clear(void)
162  {
163  freeMemory();
164  size_ = 0;
165  iteration_ = 1;
166  recentCell_ = NULL;
167  }
168 
169  void countIteration(void)
170  {
171  ++iteration_;
172  }
173 
174  std::size_t getMotionCount(void) const
175  {
176  return size_;
177  }
178 
179  std::size_t getCellCount(void) const
180  {
181  return grid_.size();
182  }
183 
185  void freeMemory(void)
186  {
187  for (typename Grid::iterator it = grid_.begin(); it != grid_.end() ; ++it)
188  freeCellData(it->second->data);
189  grid_.clear();
190  }
191 
200  unsigned int addMotion(Motion* motion, const Coord &coord, double dist = 0.0)
201  {
202  Cell *cell = grid_.getCell(coord);
203 
204  unsigned int created = 0;
205  if (cell)
206  {
207  cell->data->motions.push_back(motion);
208  cell->data->coverage += 1.0;
209  grid_.update(cell);
210  }
211  else
212  {
213  cell = grid_.createCell(coord);
214  cell->data = new CellData();
215  cell->data->motions.push_back(motion);
216  cell->data->coverage = 1.0;
217  cell->data->iteration = iteration_;
218  cell->data->selections = 1;
219  cell->data->score = (1.0 + log((double)(iteration_))) / (1.0 + dist);
220  grid_.add(cell);
221  recentCell_ = cell;
222  created = 1;
223  }
224  ++size_;
225  return created;
226  }
227 
231  void selectMotion(Motion* &smotion, Cell* &scell)
232  {
233  scell = rng_.uniform01() < std::max(selectBorderFraction_, grid_.fracExternal()) ?
234  grid_.topExternal() : grid_.topInternal();
235 
236  // We are running on finite precision, so our update scheme will end up
237  // with 0 values for the score. This is where we fix the problem
238  if (scell->data->score < std::numeric_limits<double>::epsilon())
239  {
240  std::vector<CellData*> content;
241  content.reserve(grid_.size());
242  grid_.getContent(content);
243  for (typename std::vector<CellData*>::iterator it = content.begin() ; it != content.end() ; ++it)
244  (*it)->score += 1.0 + log((double)((*it)->iteration));
245  grid_.updateAll();
246  }
247 
248  assert(scell && !scell->data->motions.empty());
249 
250  ++scell->data->selections;
251  smotion = scell->data->motions[rng_.halfNormalInt(0, scell->data->motions.size() - 1)];
252  }
253 
254  bool removeMotion(Motion *motion, const Coord &coord)
255  {
256  Cell* cell = grid_.getCell(coord);
257  if (cell)
258  {
259  bool found = false;
260  for (unsigned int i = 0 ; i < cell->data->motions.size(); ++i)
261  if (cell->data->motions[i] == motion)
262  {
263  cell->data->motions.erase(cell->data->motions.begin() + i);
264  found = true;
265  --size_;
266  break;
267  }
268  if (cell->data->motions.empty())
269  {
270  grid_.remove(cell);
271  freeCellData(cell->data);
272  grid_.destroyCell(cell);
273  }
274  return found;
275  }
276  return false;
277  }
278 
279  void updateCell(Cell *cell)
280  {
281  grid_.update(cell);
282  }
283 
284  const Grid& getGrid(void) const
285  {
286  return grid_;
287  }
288 
289  void getPlannerData(base::PlannerData &data, int tag, bool start, const Motion *lastGoalMotion) const
290  {
291  std::vector<CellData*> cdata;
292  grid_.getContent(cdata);
293 
294  if (lastGoalMotion)
295  data.addGoalVertex (base::PlannerDataVertex(lastGoalMotion->state, tag));
296 
297  for (unsigned int i = 0 ; i < cdata.size() ; ++i)
298  for (unsigned int j = 0 ; j < cdata[i]->motions.size() ; ++j)
299  {
300  if (cdata[i]->motions[j]->parent == NULL)
301  {
302  if (start)
303  data.addStartVertex(base::PlannerDataVertex(cdata[i]->motions[j]->state, tag));
304  else
305  data.addGoalVertex(base::PlannerDataVertex(cdata[i]->motions[j]->state, tag));
306  }
307  else
308  {
309  if (start)
310  data.addEdge(base::PlannerDataVertex(cdata[i]->motions[j]->parent->state, tag),
311  base::PlannerDataVertex(cdata[i]->motions[j]->state, tag));
312  else
313  data.addEdge(base::PlannerDataVertex(cdata[i]->motions[j]->state, tag),
314  base::PlannerDataVertex(cdata[i]->motions[j]->parent->state, tag));
315  }
316  }
317  }
318 
319  private:
320 
322  void freeCellData(CellData *cdata)
323  {
324  for (unsigned int i = 0 ; i < cdata->motions.size() ; ++i)
325  freeMotion_(cdata->motions[i]);
326  delete cdata;
327  }
328 
332  static void computeImportance(Cell *cell, void*)
333  {
334  CellData &cd = *(cell->data);
335  cd.importance = cd.score / ((cell->neighbors + 1) * cd.coverage * cd.selections);
336  }
337 
340  Grid grid_;
341 
344  std::size_t size_;
345 
347  unsigned int iteration_;
348 
350  Cell *recentCell_;
351 
353  FreeMotionFn freeMotion_;
354 
357  double selectBorderFraction_;
358 
360  RNG rng_;
361 
362  };
363  }
364 }
365 
366 #endif