Discretization.h

Go to the documentation of this file.

/*********************************************************************
* 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 <boost/function.hpp>
#include <vector>
#include <limits>
#include <cassert>
#include <utility>
#include <cstdlib>

namespace ompl
{

    namespace geometric
    {

        template<typename Motion>
        class Discretization
        {
        public:

            struct CellData
            {
                CellData(void) : coverage(0.0), selections(1), score(1.0), iteration(0), importance(0.0)
                {
                }

                ~CellData(void)
                {
                }

                std::vector<Motion*> motions;

                double               coverage;

                unsigned int         selections;

                double               score;

                unsigned int         iteration;

                double               importance;
            };

            struct OrderCellsByImportance
            {

                bool operator()(const CellData * const a, const CellData * const b) const
                {
                    return a->importance > b->importance;
                }
            };

            typedef GridB<CellData*, OrderCellsByImportance> Grid;

            typedef typename Grid::Cell  Cell;

            typedef typename Grid::Coord Coord;

            typedef typename boost::function1<void, Motion*> FreeMotionFn;

            Discretization(const FreeMotionFn &freeMotion) : grid_(0), size_(0), iteration_(1), recentCell_(NULL),
                                                             freeMotion_(freeMotion)
            {
                grid_.onCellUpdate(computeImportance, NULL);
                selectBorderFraction_ = 0.9;
            }

            ~Discretization(void)
            {
                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(void) const
            {
                return selectBorderFraction_;
            }

            void setDimension(unsigned int dim)
            {
                grid_.setDimension(dim);
            }

            void clear(void)
            {
                freeMemory();
                size_ = 0;
                iteration_ = 1;
                recentCell_ = NULL;
            }

            void countIteration(void)
            {
                ++iteration_;
            }

            std::size_t getMotionCount(void)
            {
                return size_;
            }

            std::size_t getCellCount(void)
            {
                return grid_.size();
            }

            void freeMemory(void)
            {
                for (typename Grid::iterator 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 (typename std::vector<CellData*>::iterator 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(void) const
            {
                return grid_;
            }

            void getPlannerData(base::PlannerData &data, int tag) const
            {
                std::vector<CellData*> cdata;
                grid_.getContent(cdata);
                for (unsigned int i = 0 ; i < cdata.size() ; ++i)
                    for (unsigned int j = 0 ; j < cdata[i]->motions.size() ; ++j)
                    {
                        data.recordEdge(cdata[i]->motions[j]->parent ? cdata[i]->motions[j]->parent->state : NULL, cdata[i]->motions[j]->state);
                        data.tagState(cdata[i]->motions[j]->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*)
            {
                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

---

ompl
Author(s): Ioan Sucan/isucan@rice.edu, Mark Moll/mmoll@rice.edu, Lydia Kavraki/kavraki@rice.edu
autogenerated on Fri Jan 11 09:33:19 2013