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