ImplicitGraph.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, Marlin Strub */
 
#ifndef OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_BITSTAR_IMPLICITGRAPH_
#define OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_BITSTAR_IMPLICITGRAPH_
 
#include "ompl/base/Cost.h"
#include "ompl/base/OptimizationObjective.h"
#include "ompl/datastructures/NearestNeighbors.h"
#include "ompl/geometric/planners/informedtrees/BITstar.h"
 
namespace ompl
{
    namespace geometric
    {

        class BITstar::ImplicitGraph
        {
        public:
            // ---
            // Construction, initialization and destruction.
            // ---

            ImplicitGraph(NameFunc nameFunc);

            virtual ~ImplicitGraph() = default;

            void setup(const ompl::base::SpaceInformationPtr &spaceInformation,
                       const ompl::base::ProblemDefinitionPtr &problemDefinition, CostHelper *costHelper,
                       SearchQueue *searchQueue, const ompl::base::Planner *plannerPtr,
                       ompl::base::PlannerInputStates &inputStates);

            void reset();
 
            // ---
            // Information access.
            // ---

            bool hasAStart() const;

            bool hasAGoal() const;

            VertexPtrVector::const_iterator startVerticesBeginConst() const;

            VertexPtrVector::const_iterator startVerticesEndConst() const;

            VertexPtrVector::const_iterator goalVerticesBeginConst() const;

            VertexPtrVector::const_iterator goalVerticesEndConst() const;

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

            bool hasInformedMeasure() const;

            double getInformedMeasure(const ompl::base::Cost &cost) const;

            double distance(const VertexConstPtr &a, const VertexConstPtr &b) const;

            double distance(const VertexConstPtrPair &vertices) const;

            void nearestSamples(const VertexPtr &vertex, VertexPtrVector *neighbourSamples);

            void getGraphAsPlannerData(ompl::base::PlannerData &data) const;

            VertexConstPtr closestVertexToGoal() const;

            double smallestDistanceToGoal() const;

            unsigned int getConnectivityK() const;

            double getConnectivityR() const;

            VertexPtrVector getCopyOfSamples() const;
 
            // ---
            // Modification.
            // ---

            void registerSolutionCost(const ompl::base::Cost &solutionCost);

            void updateStartAndGoalStates(ompl::base::PlannerInputStates &inputStates,
                                          const base::PlannerTerminationCondition &terminationCondition);

            void addNewSamples(const unsigned int &numSamples);

            std::pair<unsigned int, unsigned int> prune(double prunedMeasure);

            void addToSamples(const VertexPtr &sample);

            void addToSamples(const VertexPtrVector &samples);

            void removeFromSamples(const VertexPtr &sample);

            void pruneSample(const VertexPtr &sample);

            void recycleSample(const VertexPtr &sample);

            void registerAsVertex(const VertexPtr &vertex);

            unsigned int removeFromVertices(const VertexPtr &sample, bool moveToFree);

            std::pair<unsigned int, unsigned int> pruneVertex(const VertexPtr &vertex);

            void removeEdgeBetweenVertexAndParent(const VertexPtr &child, bool cascadeCostUpdates);
 
            // ---
            // Settings.
            // ---

            void setRewireFactor(double rewireFactor);

            double getRewireFactor() const;

            void setUseKNearest(bool useKNearest);

            bool getUseKNearest() const;

            void setJustInTimeSampling(bool useJit);

            bool getJustInTimeSampling() const;

            void setDropSamplesOnPrune(bool dropSamples);

            void setPruning(bool usePruning);

            bool getDropSamplesOnPrune() const;

            void setTrackApproximateSolutions(bool findApproximate);

            bool getTrackApproximateSolutions() const;

            void setAverageNumOfAllowedFailedAttemptsWhenSampling(std::size_t number);

            std::size_t getAverageNumOfAllowedFailedAttemptsWhenSampling() const;

            template <template <typename T> class NN>
            void setNearestNeighbors();
 
            // ---
            // Progress counters.
            // ---

            unsigned int numSamples() const;

            unsigned int numVertices() const;

            unsigned int numStatesGenerated() const;

            unsigned int numVerticesConnected() const;

            unsigned int numFreeStatesPruned() const;

            unsigned int numVerticesDisconnected() const;

            unsigned int numNearestLookups() const;

            unsigned int numStateCollisionChecks() const;
 
            // ---
            // General helper functions.
            // ---

            bool canVertexBeDisconnected(const VertexPtr &vertex) const;

            bool canSampleBePruned(const VertexPtr &sample) const;
 
        private:
            // ---
            // High-level primitives updating the graph.
            // ---

            void updateSamples(const VertexConstPtr &vertex);

            void updateVertexClosestToGoal();
 
            // ---
            // High-level primitives pruning the graph.
            // ---

            std::pair<unsigned int, unsigned int> pruneStartAndGoalVertices();

            std::pair<unsigned int, unsigned int> pruneSamples();
 
            // ---
            // Low-level random geometric graph helper and calculations
            // ---

            void testClosestToGoal(const VertexConstPtr &vertex);

            ompl::base::Cost calculateNeighbourhoodCost(const VertexConstPtr &vertex) const;

            virtual void updateNearestTerms();

            std::size_t computeNumberOfSamplesInInformedSet() const;

            double calculateR(unsigned int numUniformSamples) const;

            unsigned int calculateK(unsigned int numUniformSamples) const;

            double calculateMinimumRggR() const;

            double calculateMinimumRggK() const;
 
            // ---
            // Debug helpers.
            // ---

            void assertSetup() const;
 
            // ---
            // Member variables (Make all are configured in setup() and reset in reset()).
            // ---

            NameFunc nameFunc_;

            bool isSetup_{false};

            ompl::base::SpaceInformationPtr spaceInformation_{nullptr};

            ompl::base::ProblemDefinitionPtr problemDefinition_{nullptr};

            CostHelper *costHelpPtr_{nullptr};

            SearchQueue *queuePtr_{nullptr};

            ompl::RNG rng_;

            ompl::base::InformedSamplerPtr sampler_{nullptr};

            VertexPtrVector startVertices_;

            VertexPtrVector goalVertices_;

            VertexPtrVector prunedStartVertices_;

            VertexPtrVector prunedGoalVertices_;

            VertexPtrVector newSamples_;

            VertexPtrNNPtr samples_{nullptr};

            VertexPtrVector recycledSamples_;

            unsigned int numNewSamplesInCurrentBatch_{0u};

            unsigned int numUniformStates_{0u};

            double r_{0.};

            double k_rgg_{0.};

            unsigned int k_{0u};

            double approximationMeasure_{0.};

            ompl::base::Cost minCost_{std::numeric_limits<double>::infinity()};

            ompl::base::Cost solutionCost_{std::numeric_limits<double>::infinity()};

            ompl::base::Cost sampledCost_{std::numeric_limits<double>::infinity()};

            bool hasExactSolution_{false};

            VertexConstPtr closestVertexToGoal_{nullptr};

            double closestDistanceToGoal_{std::numeric_limits<double>::infinity()};

            unsigned int numSamples_{0u};

            unsigned int numVertices_{0u};

            unsigned int numFreeStatesPruned_{0u};

            unsigned int numVerticesDisconnected_{0u};

            unsigned int numNearestNeighbours_{0u};

            unsigned int numStateCollisionChecks_{0u};

            const std::shared_ptr<unsigned int> approximationId_;
 
            // ---
            // Parameters - Set defaults in construction/setup and do not reset in clear.
            // ---

            double rewireFactor_{1.1};

            bool useKNearest_{true};

            bool useJustInTimeSampling_{false};

            bool dropSamplesOnPrune_{false};

            bool isPruningEnabled_{true};

            bool findApprox_{false};

            std::size_t averageNumOfAllowedFailedAttemptsWhenSampling_{2u};
        };  // class ImplicitGraph
    }  // namespace geometric
}  // namespace ompl
 
#endif  // OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_BITSTAR_IMPLICITGRAPH_