IntegratedQueue.h

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2014, University of Toronto
*  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 University of Toronto 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.
*********************************************************************/

/* Authors: Jonathan Gammell */

#ifndef OMPL_GEOMETRIC_PLANNERS_BITSTAR_DATASTRUCTURES_INTEGRATEDQUEUE_
#define OMPL_GEOMETRIC_PLANNERS_BITSTAR_DATASTRUCTURES_INTEGRATEDQUEUE_

//STL/Boost/etc.:
//std::pair
#include <utility>
//std::vector
#include <vector>
//std::list
#include <list>
//std::multimap
#include <map>
//std::unordered_map
#include <unordered_map>
//For std::function
#include <functional>

//OMPL:
//The cost class:
#include "ompl/base/Cost.h"
//The optimization objective class:
#include "ompl/base/OptimizationObjective.h"
//The nearest neighbours structure
#include "ompl/datastructures/NearestNeighbors.h"

//BIT*:
//I am member class of the BITstar class, so I need to include it's definition to be aware of the class BITstar. It has a forward declaration to me.
#include "ompl/geometric/planners/bitstar/BITstar.h"
//The vertex class:
#include "ompl/geometric/planners/bitstar/datastructures/Vertex.h"

namespace ompl
{
    namespace geometric
    {


        class BITstar::IntegratedQueue
        {
        public:
            //Data typedefs:
            typedef std::pair<ompl::base::Cost, ompl::base::Cost> CostPair;

            //Function typedefs:
            typedef std::function<ompl::base::Cost (const VertexConstPtr&)> VertexHeuristicFunc;

            typedef std::function<ompl::base::Cost (const VertexConstPtrPair&)> EdgeHeuristicFunc;

            typedef std::function<double (const VertexConstPtr&, const VertexConstPtr&)> DistanceFunc;

            typedef std::function<unsigned int (const VertexPtr&, std::vector<VertexPtr>*)> NeighbourhoodFunc;



            //Public functions:
            //std::make_shared can only take 9 arguments, so be careful:
            IntegratedQueue(const ompl::base::OptimizationObjectivePtr& opt, const DistanceFunc& distanceFunc, const NeighbourhoodFunc& nearSamplesFunc, const NeighbourhoodFunc& nearVerticesFunc, const VertexHeuristicFunc& lowerBoundHeuristicVertex, const VertexHeuristicFunc& currentHeuristicVertex, const EdgeHeuristicFunc& lowerBoundHeuristicEdge, const EdgeHeuristicFunc& currentHeuristicEdge, const EdgeHeuristicFunc& currentHeuristicEdgeTarget);

            virtual ~IntegratedQueue();

            void setDelayedRewiring(bool delayRewiring);

            bool getDelayedRewiring() const;

            //Insert and erase
            void insertVertex(const VertexPtr& newVertex);

            void insertEdge(const VertexPtrPair& newEdge);

            void eraseVertex(const VertexPtr& oldVertex, bool disconnectParent, const VertexPtrNNPtr& vertexNN, const VertexPtrNNPtr& freeStateNN, std::vector<VertexPtr>* recycledVertices);

            //Access the queue
            VertexPtr frontVertex();

            VertexPtrPair frontEdge();

            ompl::base::Cost frontVertexValue();

            CostPair frontEdgeValue();

            void popFrontEdge(VertexPtrPair* bestEdge);

            VertexPtrPair popFrontEdge();

            //Queue maintenance
            void hasSolution();

            void setThreshold(const ompl::base::Cost& costThreshold);

            ompl::base::Cost getThreshold() const;

            void removeEdgesTo(const VertexPtr& cVertex);

            void removeEdgesFrom(const VertexPtr& pVertex);

            void pruneEdgesTo(const VertexPtr& cVertex);

            void pruneEdgesFrom(const VertexPtr& pVertex);

            void markVertexUnsorted(const VertexPtr& vertex);

            std::pair<unsigned int, unsigned int> prune(const VertexPtr& pruneStartPtr, const VertexPtrNNPtr& vertexNN, const VertexPtrNNPtr& freeStateNN, std::vector<VertexPtr>* recycledVertices);

            std::pair<unsigned int, unsigned int> resort(const VertexPtrNNPtr& vertexNN, const VertexPtrNNPtr& freeStateNN, std::vector<VertexPtr>* recycledVertices);

            void finish();

            void reset();

            void clear();

            //Queue info:
            bool vertexPruneCondition(const VertexPtr& vertex) const;

            bool samplePruneCondition(const VertexPtr& vertex) const;

            bool edgePruneCondition(const VertexPtrPair& edge) const;

            unsigned int numEdges() const;

            unsigned int numVertices() const;

            unsigned int numEdgesTo(const VertexPtr& cVertex) const;

            unsigned int numEdgesFrom(const VertexPtr& pVertex) const;

            bool isSorted() const;

            bool isReset() const;

            bool isEmpty();

            bool isVertexExpanded(const VertexConstPtr& vertex) const;

            void listVertices(std::vector<VertexConstPtr>* vertexQueue);

            void listEdges(std::vector<std::pair<VertexConstPtr, VertexConstPtr> >* edgeQueue);




        private:
            //Helpful typedefs:
            typedef std::multimap<ompl::base::Cost, VertexPtr, std::function<bool (const ompl::base::Cost&, const ompl::base::Cost&)> > CostToVertexMMap;

            typedef std::multimap<CostPair, VertexPtrPair, std::function<bool (const CostPair&, const CostPair&)> > CostToVertexPtrPairMMap;

            typedef CostToVertexMMap::iterator VertexQueueIter;

            typedef std::unordered_map<BITstar::VertexId, VertexQueueIter> VertexIdToVertexQueueIterUMap;

            typedef CostToVertexPtrPairMMap::iterator EdgeQueueIter;

            typedef std::list<EdgeQueueIter> EdgeQueueIterList;

            typedef std::unordered_map<BITstar::VertexId, EdgeQueueIterList> VertexIdToEdgeQueueIterListUMap;


            //High level primitives:
            void updateQueue();

            void expandNextVertex();

            void expandVertex(const VertexPtr& vertex);

            void queueupEdge(const VertexPtr& parent, const VertexPtr& child);

            void processKNearest(unsigned int k, const VertexConstPtr& vertex, std::vector<VertexPtr>* kNearSamples, std::vector<VertexPtr>* kNearVertices);


            //Vertex helper functions:
            void reinsertVertex(const VertexPtr& unorderedVertex);

            std::pair<unsigned int, unsigned int> pruneBranch(const VertexPtr& branchBase, const VertexPtrNNPtr& vertexNN, const VertexPtrNNPtr& freeStateNN, std::vector<VertexPtr>* recycledVertices);

            void disconnectParent(const VertexPtr& oldVertex, bool cascadeCostUpdates);

            void vertexInsertHelper(const VertexPtr& newVertex, bool expandIfBeforeToken);

            //This is *NOT* by const-reference so that the oldVertex pointer doesn't go out of scope on me... which was happening if it was being called with an iter->second where the iter gets deleted in this function...
            unsigned int vertexRemoveHelper(VertexPtr oldVertex, const VertexPtrNNPtr& vertexNN, const VertexPtrNNPtr& freeStateNN, std::vector<VertexPtr>* recycledVertices, bool removeLookups);

            //Edge helper functions:
            void edgeInsertHelper(const VertexPtrPair& newEdge, EdgeQueueIter positionHint);

            void edgeRemoveHelper(const EdgeQueueIter& oldEdgeIter, bool rmIncomingLookup, bool rmOutgoingLookup);

            void rmIncomingLookup(const EdgeQueueIter& mmapIterToRm);

            void rmOutgoingLookup(const EdgeQueueIter& mmapIterToRm);

            void rmEdgeLookupHelper(VertexIdToEdgeQueueIterListUMap& lookup, const BITstar::VertexId& idx, const EdgeQueueIter& mmapIterToRm);




            //Base-queue basic helper functions:
            ompl::base::Cost vertexQueueValue(const VertexPtr& vertex) const;

            CostPair edgeQueueValue(const VertexPtrPair& edge) const;

            bool vertexQueueComparison(const ompl::base::Cost& lhs, const ompl::base::Cost& rhs) const;

            bool edgeQueueComparison(const CostPair& lhs, const CostPair& rhs) const;

            //Cost helpers
            bool isCostWorseThan(const ompl::base::Cost& a, const ompl::base::Cost& b) const;

            bool isCostNotEquivalentTo(const ompl::base::Cost& a, const ompl::base::Cost& b) const;

            bool isCostBetterThanOrEquivalentTo(const ompl::base::Cost& a, const ompl::base::Cost& b) const;

            bool isCostWorseThanOrEquivalentTo(const ompl::base::Cost& a, const ompl::base::Cost& b) const;



            //Member variables:
            ompl::base::OptimizationObjectivePtr                     opt_;

            DistanceFunc                                             distanceFunc_;

            NeighbourhoodFunc                                        nearSamplesFunc_;

            NeighbourhoodFunc                                        nearVerticesFunc_;

            VertexHeuristicFunc                                      lowerBoundHeuristicVertexFunc_;

            VertexHeuristicFunc                                      currentHeuristicVertexFunc_;

            EdgeHeuristicFunc                                        lowerBoundHeuristicEdgeFunc_;

            EdgeHeuristicFunc                                        currentHeuristicEdgeFunc_;

            EdgeHeuristicFunc                                        currentHeuristicEdgeTargetFunc_;

            bool                                                     delayRewiring_;

            bool                                                     outgoingLookupTables_;

            bool                                                     incomingLookupTables_;

            CostToVertexMMap                                         vertexQueue_;

            VertexQueueIter                                          vertexToExpand_;

            CostToVertexPtrPairMMap                                  edgeQueue_;

            VertexIdToVertexQueueIterUMap                            vertexIterLookup_;

            VertexIdToEdgeQueueIterListUMap                          outgoingEdges_;

            VertexIdToEdgeQueueIterListUMap                          incomingEdges_;

            std::list<VertexPtr>                                     resortVertices_;

            ompl::base::Cost                                         costThreshold_;

            bool                                                     hasSolution_;
        }; //class: IntegratedQueue
    } //geometric
} //ompl
#endif //OMPL_GEOMETRIC_PLANNERS_BITSTAR_DATASTRUCTURES_INTEGRATEDQUEUE_