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_