37 #ifndef OMPL_GEOMETRIC_PLANNERS_KPIECE_DISCRETIZATION_
38 #define OMPL_GEOMETRIC_PLANNERS_KPIECE_DISCRETIZATION_
40 #include "ompl/base/Planner.h"
41 #include "ompl/datastructures/GridB.h"
42 #include "ompl/util/Exception.h"
43 #include <boost/function.hpp>
58 template<
typename Motion>
123 freeMotion_(freeMotion)
126 selectBorderFraction_ = 0.9;
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;
151 return selectBorderFraction_;
169 void countIteration(
void)
174 std::size_t getMotionCount(
void)
const
179 std::size_t getCellCount(
void)
const
188 freeCellData(it->second->data);
204 unsigned int created = 0;
207 cell->
data->motions.push_back(motion);
208 cell->
data->coverage += 1.0;
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);
238 if (scell->
data->score < std::numeric_limits<double>::epsilon())
240 std::vector<CellData*> content;
241 content.reserve(grid_.
size());
243 for (
typename std::vector<CellData*>::iterator it = content.begin() ; it != content.end() ; ++it)
244 (*it)->score += 1.0 +
log((
double)((*it)->iteration));
248 assert(scell && !scell->
data->motions.empty());
250 ++scell->
data->selections;
254 bool removeMotion(Motion *motion,
const Coord &coord)
260 for (
unsigned int i = 0 ; i < cell->data->motions.size(); ++i)
261 if (cell->data->motions[i] == motion)
263 cell->
data->motions.erase(cell->data->motions.begin() + i);
268 if (cell->data->motions.empty())
271 freeCellData(cell->data);
279 void updateCell(
Cell *cell)
284 const Grid& getGrid(
void)
const
289 void getPlannerData(base::PlannerData &data,
int tag,
bool start,
const Motion *lastGoalMotion)
const
291 std::vector<CellData*> cdata;
295 data.addGoalVertex (base::PlannerDataVertex(lastGoalMotion->state, tag));
297 for (
unsigned int i = 0 ; i < cdata.size() ; ++i)
298 for (
unsigned int j = 0 ; j < cdata[i]->motions.size() ; ++j)
300 if (cdata[i]->motions[j]->parent == NULL)
303 data.addStartVertex(base::PlannerDataVertex(cdata[i]->motions[j]->state, tag));
305 data.addGoalVertex(base::PlannerDataVertex(cdata[i]->motions[j]->state, tag));
310 data.addEdge(base::PlannerDataVertex(cdata[i]->motions[j]->parent->state, tag),
311 base::PlannerDataVertex(cdata[i]->motions[j]->state, tag));
313 data.addEdge(base::PlannerDataVertex(cdata[i]->motions[j]->state, tag),
314 base::PlannerDataVertex(cdata[i]->motions[j]->parent->state, tag));
322 void freeCellData(CellData *cdata)
324 for (
unsigned int i = 0 ; i < cdata->motions.size() ; ++i)
325 freeMotion_(cdata->motions[i]);
332 static void computeImportance(
Cell *cell,
void*)
334 CellData &cd = *(cell->data);
335 cd.importance = cd.score / ((cell->neighbors + 1) * cd.coverage * cd.selections);
347 unsigned int iteration_;
357 double selectBorderFraction_;