Program Listing for File Discretization.h
↰ Return to documentation for file (src/ompl/geometric/planners/kpiece/Discretization.h)
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Rice University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Rice University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Ioan Sucan */
#ifndef OMPL_GEOMETRIC_PLANNERS_KPIECE_DISCRETIZATION_
#define OMPL_GEOMETRIC_PLANNERS_KPIECE_DISCRETIZATION_
#include "ompl/base/Planner.h"
#include "ompl/datastructures/GridB.h"
#include "ompl/util/Exception.h"
#include <functional>
#include <utility>
#include <vector>
#include <limits>
#include <cassert>
#include <utility>
#include <cstdlib>
#include <cmath>
namespace ompl
{
namespace geometric
{
template <typename Motion>
class Discretization
{
public:
struct CellData
{
CellData() = default;
~CellData() = default;
std::vector<Motion *> motions;
double coverage{0.0};
unsigned int selections{1};
double score{1.0};
unsigned int iteration{0};
double importance{0.0};
};
struct OrderCellsByImportance
{
bool operator()(const CellData *const a, const CellData *const b) const
{
return a->importance > b->importance;
}
};
using Grid = GridB<CellData *, OrderCellsByImportance>;
using Cell = typename Grid::Cell;
using Coord = typename Grid::Coord;
using FreeMotionFn = typename std::function<void(Motion *)>;
Discretization(FreeMotionFn freeMotion)
: grid_(0), size_(0), iteration_(1), recentCell_(nullptr), freeMotion_(std::move(freeMotion))
{
grid_.onCellUpdate(computeImportance, nullptr);
selectBorderFraction_ = 0.9;
}
~Discretization()
{
freeMemory();
}
void setBorderFraction(double bp)
{
if (bp < std::numeric_limits<double>::epsilon() || bp > 1.0)
throw Exception("The fraction of time spent selecting border cells must be in the range (0,1]");
selectBorderFraction_ = bp;
}
double getBorderFraction() const
{
return selectBorderFraction_;
}
void setDimension(unsigned int dim)
{
grid_.setDimension(dim);
}
void clear()
{
freeMemory();
size_ = 0;
iteration_ = 1;
recentCell_ = nullptr;
}
void countIteration()
{
++iteration_;
}
std::size_t getMotionCount() const
{
return size_;
}
std::size_t getCellCount() const
{
return grid_.size();
}
void freeMemory()
{
for (auto it = grid_.begin(); it != grid_.end(); ++it)
freeCellData(it->second->data);
grid_.clear();
}
unsigned int addMotion(Motion *motion, const Coord &coord, double dist = 0.0)
{
Cell *cell = grid_.getCell(coord);
unsigned int created = 0;
if (cell)
{
cell->data->motions.push_back(motion);
cell->data->coverage += 1.0;
grid_.update(cell);
}
else
{
cell = grid_.createCell(coord);
cell->data = new CellData();
cell->data->motions.push_back(motion);
cell->data->coverage = 1.0;
cell->data->iteration = iteration_;
cell->data->selections = 1;
cell->data->score = (1.0 + log((double)(iteration_))) / (1.0 + dist);
grid_.add(cell);
recentCell_ = cell;
created = 1;
}
++size_;
return created;
}
void selectMotion(Motion *&smotion, Cell *&scell)
{
scell = rng_.uniform01() < std::max(selectBorderFraction_, grid_.fracExternal()) ? grid_.topExternal() :
grid_.topInternal();
// We are running on finite precision, so our update scheme will end up
// with 0 values for the score. This is where we fix the problem
if (scell->data->score < std::numeric_limits<double>::epsilon())
{
std::vector<CellData *> content;
content.reserve(grid_.size());
grid_.getContent(content);
for (auto it = content.begin(); it != content.end(); ++it)
(*it)->score += 1.0 + log((double)((*it)->iteration));
grid_.updateAll();
}
assert(scell && !scell->data->motions.empty());
++scell->data->selections;
smotion = scell->data->motions[rng_.halfNormalInt(0, scell->data->motions.size() - 1)];
}
bool removeMotion(Motion *motion, const Coord &coord)
{
Cell *cell = grid_.getCell(coord);
if (cell)
{
bool found = false;
for (unsigned int i = 0; i < cell->data->motions.size(); ++i)
if (cell->data->motions[i] == motion)
{
cell->data->motions.erase(cell->data->motions.begin() + i);
found = true;
--size_;
break;
}
if (cell->data->motions.empty())
{
grid_.remove(cell);
freeCellData(cell->data);
grid_.destroyCell(cell);
}
return found;
}
return false;
}
void updateCell(Cell *cell)
{
grid_.update(cell);
}
const Grid &getGrid() const
{
return grid_;
}
void getPlannerData(base::PlannerData &data, int tag, bool start, const Motion *lastGoalMotion) const
{
std::vector<CellData *> cdata;
grid_.getContent(cdata);
if (lastGoalMotion)
data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion->state, tag));
for (unsigned int i = 0; i < cdata.size(); ++i)
for (unsigned int j = 0; j < cdata[i]->motions.size(); ++j)
{
if (cdata[i]->motions[j]->parent == nullptr)
{
if (start)
data.addStartVertex(base::PlannerDataVertex(cdata[i]->motions[j]->state, tag));
else
data.addGoalVertex(base::PlannerDataVertex(cdata[i]->motions[j]->state, tag));
}
else
{
if (start)
data.addEdge(base::PlannerDataVertex(cdata[i]->motions[j]->parent->state, tag),
base::PlannerDataVertex(cdata[i]->motions[j]->state, tag));
else
data.addEdge(base::PlannerDataVertex(cdata[i]->motions[j]->state, tag),
base::PlannerDataVertex(cdata[i]->motions[j]->parent->state, tag));
}
}
}
private:
void freeCellData(CellData *cdata)
{
for (unsigned int i = 0; i < cdata->motions.size(); ++i)
freeMotion_(cdata->motions[i]);
delete cdata;
}
static void computeImportance(Cell *cell, void * /*unused*/)
{
CellData &cd = *(cell->data);
cd.importance = cd.score / ((cell->neighbors + 1) * cd.coverage * cd.selections);
}
Grid grid_;
std::size_t size_;
unsigned int iteration_;
Cell *recentCell_;
FreeMotionFn freeMotion_;
double selectBorderFraction_;
RNG rng_;
};
}
}
#endif