Class BITstar

Defined in File BITstar.h

Nested Relationships

Nested Types

Inheritance Relationships

Base Type

public ompl::base::Planner (Class Planner)

Derived Type

public ompl::geometric::ABITstar (Class ABITstar)

Class Documentation

class BITstar : public ompl::base::Planner 

Batch Informed Trees (BIT*)

BIT* (Batch Informed Trees) is an anytime asymptotically optimal sampling-based planning algorithm. It approaches problems by assuming that a simple solution exists and only goes onto consider complex solutions when that proves incorrect. It accomplishes this by using heuristics to search in order of decreasing potential solution quality.

Both a k-nearest and r-disc version are available, with the k-nearest selected by default. In general, the r-disc variant considers more connections than the k-nearest. For a small number of specific planning problems, this results in it finding solutions slower than k-nearest (hence the default choice). It is recommended that you try both variants, with the r-disc version being recommended if it finds an initial solution in a suitable amount of time (which it probably will). The difference in this number of connections considered is a RGG theory question, and certainly merits further review.

This implementation of BIT* can handle multiple starts, multiple goals, a variety of optimization objectives (e.g., path length), and with just-in-time sampling, infinite problem domains. Note that for some of optimization objectives, the user must specify a suitable heuristic and that when this heuristic is not specified, it will use the conservative/always admissible zero-heuristic.

This implementation also includes some new advancements, including the ability to prioritize exploration until an initial solution is found (Delayed rewiring), the ability to generate samples only when necessary (Just-in-time sampling), and the ability to periodically remove samples that have yet to be connected to the graph (Sample dropping). With just-in-time sampling, BIT* can even solve planning problems with infinite state space boundaries, i.e., (-inf, inf).

J. D. Gammell, T. D. Barfoot, S. S. Srinivasa, “Batch Informed Trees (BIT*): Informed asymptotically optimal

anytime search.” The International Journal of Robotics Research (IJRR), 39(5): 543-567, Apr. 2020. DOI:

10.1177/0278364919890396. arXiv: 1707.01888 [cs.RO]. Illustration video.

Associated publication:

Subclassed by ompl::geometric::ABITstar

Public Types

using VertexPtr = std::shared_ptr<Vertex>: A shared pointer to a vertex.

using VertexConstPtr = std::shared_ptr<const Vertex>: A shared pointer to a const vertex.

using VertexWeakPtr = std::weak_ptr<Vertex>: A weak pointer to a vertex.

using VertexPtrVector = std::vector<VertexPtr>: A vector of shared pointers to vertices.

using VertexConstPtrVector = std::vector<VertexConstPtr>: A vector of shared pointers to const vertices.

using VertexId = unsigned int: The vertex id type.

using VertexPtrPair = std::pair<VertexPtr, VertexPtr>: A pair of vertices, i.e., an edge.

using VertexConstPtrPair = std::pair<VertexConstPtr, VertexConstPtr>: A pair of const vertices, i.e., an edge.

using VertexPtrPairVector = std::vector<VertexPtrPair>: A vector of pairs of vertices, i.e., a vector of edges.

using VertexConstPtrPairVector = std::vector<VertexConstPtrPair>: A vector of pairs of const vertices, i.e., a vector of edges.

using VertexPtrNNPtr = std::shared_ptr<NearestNeighbors<VertexPtr>>: The OMPL::NearestNeighbors structure.

using NameFunc = std::function<std::string()>: A utility functor for ImplicitGraph and SearchQueue.

Public Functions

BITstar(const base::SpaceInformationPtr &spaceInfo, const std::string &name = "kBITstar"): Construct with a pointer to the space information and an optional name.

virtual ~BITstar() override = default: Destruct using the default destructor.

virtual void setup() override: Setup the algorithm.

virtual void clear() override: Clear the algorithm’s internal state.

virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &terminationCondition) override: Solve the problem given a termination condition.

virtual void getPlannerData(base::PlannerData &data) const override: Get results.

std::pair<const ompl::base::State*, const ompl::base::State*> getNextEdgeInQueue(): Get the next edge to be processed. Causes vertices in the queue to be expanded (if necessary) and therefore effects the run timings of the algorithm, but helpful for some videos and debugging.

ompl::base::Cost getNextEdgeValueInQueue(): Get the value of the next edge to be processed. Causes vertices in the queue to be expanded (if necessary) and therefore effects the run timings of the algorithm, but helpful for some videos and debugging.

void getEdgeQueue(VertexConstPtrPairVector *edgesInQueue): Get the whole messy set of edges in the queue. Expensive but helpful for some videos.

unsigned int numIterations() const: Get the number of iterations completed.

ompl::base::Cost bestCost() const: Retrieve the best exact-solution cost found.

unsigned int numBatches() const: Retrieve the number of batches processed as the raw data.

void setRewireFactor(double rewireFactor): Set the rewiring scale factor, s, such that r_rrg = s \times r_rrg*.

double getRewireFactor() const: Get the rewiring scale factor.

void setAverageNumOfAllowedFailedAttemptsWhenSampling(std::size_t number): Set the average number of allowed failed attempts when sampling.

std::size_t getAverageNumOfAllowedFailedAttemptsWhenSampling() const: Get the average number of allowed failed attempts when sampling.

void setSamplesPerBatch(unsigned int n): Set the number of samplers per batch.

unsigned int getSamplesPerBatch() const: Get the number of samplers per batch.

void setUseKNearest(bool useKNearest): Enable a k-nearest search for instead of an r-disc search.

bool getUseKNearest() const: Get whether a k-nearest search is being used.

void setStrictQueueOrdering(bool beStrict): Enable “strict sorting” of the edge queue. Rewirings can change the position in the queue of an edge. When strict sorting is enabled, the effected edges are resorted immediately, while disabling strict sorting delays this resorting until the end of the batch.

bool getStrictQueueOrdering() const: Get whether strict queue ordering is in use.

void setPruning(bool prune): Enable pruning of vertices/samples that CANNOT improve the current solution. When a vertex in the graph is pruned, it’s descendents are also pruned (if they also cannot improve the solution) or placed back in the set of free samples (if they could improve the solution). This assures that a uniform density is maintained.

bool getPruning() const: Get whether graph and sample pruning is in use.

void setPruneThresholdFraction(double fractionalChange): Set the fractional change in the solution cost AND problem measure necessary for pruning to occur.

double getPruneThresholdFraction() const: Get the fractional change in the solution cost AND problem measure necessary for pruning to occur.

void setDelayRewiringUntilInitialSolution(bool delayRewiring)

Delay the consideration of rewiring edges until an initial solution is found. When multiple batches are required to find an initial solution, this can improve the time required to do so, by delaying improvements in the cost-to-come to a connected vertex. As the rewiring edges are considered once an initial solution is found, this has no effect on the theoretical asymptotic optimality of the planner.

bool getDelayRewiringUntilInitialSolution() const: Get whether BIT* is delaying rewiring until a solution is found.

void setJustInTimeSampling(bool useJit)

Delay the generation of samples until they are necessary. This only works when using an r-disc connection scheme, and is currently only implemented for problems seeking to minimize path length. This helps reduce the complexity of nearest-neighbour look ups, and can be particularly beneficial in unbounded planning problems where selecting an appropriate bounding box is difficult. With JIT sampling enabled, BIT* can solve planning problems whose state space has infinite boundaries. When enumerating outgoing edges from a vertex, BIT* uses JIT sampling to assure that the area within r of the vertex has been sampled during this batch. This is done in a way that maintains uniform sample distribution and has no effect on the theoretical asymptotic optimality of the planner.

bool getJustInTimeSampling() const: Get whether we’re using just-in-time sampling.

void setDropSamplesOnPrune(bool dropSamples)

Drop all unconnected samples when pruning, regardless of their heuristic value. This provides a method for BIT* to remove samples that have not been connected to the graph and may be beneficial in problems where portions of the free space are unreachable (i.e., disconnected). BIT* calculates the connection radius for each batch from the underlying uniform distribution of states. The resulting larger connection radius may be detrimental in areas where the graph is dense, but maintains the theoretical asymptotic optimality of the planner.

bool getDropSamplesOnPrune() const: Get whether unconnected samples are dropped on pruning.

void setStopOnSolnImprovement(bool stopOnChange): Stop the planner each time a solution improvement is found. Useful for examining the intermediate solutions found by BIT*.

bool getStopOnSolnImprovement() const: Get whether BIT* stops each time a solution is found.

void setConsiderApproximateSolutions(bool findApproximate): Set BIT* to consider approximate solutions during its initial search.

bool getConsiderApproximateSolutions() const: Get whether BIT* is considering approximate solutions.

template<template<typename T> class NN> void setNearestNeighbors(): Set a different nearest neighbours datastructure.

Protected Functions

void setInitialInflationFactor(double factor): Set the inflation for the initial search of RGG approximation. See ABIT*’s class description for more details about the inflation factor update policy.

void setInflationScalingParameter(double parameter): The parameter that scales the inflation factor on the second search of each RGG approximation. See ABIT*’s class description for more details about the inflation factor update policy.

void setTruncationScalingParameter(double parameter): Sets the parameter that scales the truncation factor for the searches of each RGG approximation. See ABIT*’s class description for more details about the truncation factor update policy.

void enableCascadingRewirings(bool enable): Enable the cascading of rewirings.

double getInitialInflationFactor() const: Get the inflation factor for the initial search.

double getInflationScalingParameter() const: Get the inflation scaling parameter.

double getTruncationScalingParameter() const: Get the truncation factor parameter.

double getCurrentInflationFactor() const: Get the inflation factor for the current search.

double getCurrentTruncationFactor() const: Get the truncation factor for the current search.

class CostHelper

A helper class to handle the various heuristic functions in one place.

Short Description: A header-only class that consolidates all the various heuristic calculations for vertices/edges in a graph into one place. Most of these functions are simply combinatorial pass-throughs to the OptimizationObjective.

Public Functions

CostHelper() = default: Construct the heuristic helper, must be setup before use.

virtual ~CostHelper() = default

inline void setup(const ompl::base::OptimizationObjectivePtr &opt, ImplicitGraph *graph): Setup the CostHelper, must be called before use.

inline void reset(): Reset the CostHelper, returns to state at construction.

inline ompl::base::OptimizationObjectivePtr getOptObj() const: Get the underling OptimizationObjective.

inline ompl::base::Cost lowerBoundHeuristicVertex(const VertexConstPtr &vertex) const: Calculates a heuristic estimate of the cost of a solution constrained to pass through a vertex, independent of the current cost-to-come. I.e., combines the heuristic estimates of the cost-to-come and cost-to-go.

inline ompl::base::Cost currentHeuristicVertex(const VertexConstPtr &vertex) const: Calculates a heuristic estimate of the cost of a solution constrained to pass through a vertex, dependent on the current cost-to-come. I.e., combines the current cost-to-come with a heuristic estimate of the cost-to-go.

inline ompl::base::Cost lowerBoundHeuristicEdge(const VertexConstPtrPair &edgePair) const: Calculates a heuristic estimate of the cost of a solution constrained to go through an edge, independent of the cost-to-come of the parent state. I.e., combines the heuristic estimates of the cost-to-come, edge cost, and cost-to-go.

inline ompl::base::Cost currentHeuristicEdge(const VertexConstPtrPair &edgePair) const: Calculates a heuristic estimate of the cost of a solution constrained to go through an edge, dependent on the cost-to-come of the parent state. I.e., combines the current cost-to-come with heuristic estimates of the edge cost, and cost-to-go.

inline ompl::base::Cost lowerBoundHeuristicToTarget(const VertexConstPtrPair &edgePair) const: Calculates a heuristic estimate of the cost of a path to the target of an edge, independent of the current cost-to-come of the parent state. I.e., combines heuristics estimates of the cost-to-come and the edge cost.

inline ompl::base::Cost currentHeuristicToTarget(const VertexConstPtrPair &edgePair) const: Calculates a heuristic estimate of the cost of a path to the target of an edge, dependent on the cost-to-come of the parent state. I.e., combines the current cost-to-come with heuristic estimates of the edge cost.

inline ompl::base::Cost costToComeHeuristic(const VertexConstPtr &vertex) const: Calculate a heuristic estimate of the cost-to-come for a Vertex.

inline ompl::base::Cost edgeCostHeuristic(const VertexConstPtrPair &edgePair) const: Calculate a heuristic estimate of the cost of an edge between two Vertices.

inline ompl::base::Cost costToGoHeuristic(const VertexConstPtr &vertex) const: Calculate a heuristic estimate of the cost-to-go for a Vertex.

inline ompl::base::Cost trueEdgeCost(const VertexConstPtrPair &edgePair) const: The true cost of an edge, including constraints.

template<typename ...Costs> inline ompl::base::Cost combineCosts(const ompl::base::Cost &cost, const Costs&... costs) const: Combine multiple costs.

inline ompl::base::Cost inflateCost(const ompl::base::Cost &cost, double factor) const: Inflate a cost by a given factor.

inline bool isCostWorseThan(const ompl::base::Cost &a, const ompl::base::Cost &b) const: Compare whether cost a is worse than cost b by checking whether b is better than a.

inline bool isCostNotEquivalentTo(const ompl::base::Cost &a, const ompl::base::Cost &b) const: Compare whether cost a and cost b are not equivalent by checking if either a or b is better than the other.

inline bool isCostBetterThanOrEquivalentTo(const ompl::base::Cost &a, const ompl::base::Cost &b) const: Compare whether cost a is better or equivalent to cost b by checking that b is not better than a.

inline bool isCostWorseThanOrEquivalentTo(const ompl::base::Cost &a, const ompl::base::Cost &b) const: Compare whether cost a is worse or equivalent to cost b by checking that a is not better than b.

inline double fractionalChange(const ompl::base::Cost &newCost, const ompl::base::Cost &oldCost) const: Calculate the fractional change of cost “newCost” from “oldCost” relative to “oldCost”, i.e., (newCost - oldCost)/oldCost.

inline double fractionalChange(const ompl::base::Cost &newCost, const ompl::base::Cost &oldCost, const ompl::base::Cost &refCost) const: Calculate the fractional change of cost “newCost” from “oldCost” relative to “refCost”, i.e., (newCost - oldCost)/refCost.

inline bool isSatisfied(const ompl::base::Cost &a) const

inline bool isFinite(const ompl::base::Cost &a) const

inline bool isCostEquivalentTo(const ompl::base::Cost &a, const ompl::base::Cost &b) const

inline bool isCostBetterThan(const ompl::base::Cost &a, const ompl::base::Cost &b) const

inline ompl::base::Cost betterCost(const ompl::base::Cost &a, const ompl::base::Cost &b) const

inline ompl::base::Cost combineCosts(const ompl::base::Cost &a, const ompl::base::Cost &b) const

inline ompl::base::Cost infiniteCost() const

inline ompl::base::Cost identityCost() const

inline ompl::base::Cost motionCostHeuristic(const ompl::base::State *a, const ompl::base::State *b) const

inline ompl::base::Cost motionCost(const ompl::base::State *a, const ompl::base::State *b) const

class IdGenerator

An ID generator class for vertex IDs.

Short description: A class to generate unique IDs for the Vertex class.

Public Functions

IdGenerator() = default

inline BITstar::VertexId getNewId(): Generator a new id and increment the global/static counter of IDs.

class ImplicitGraph

A conceptual representation of samples as an edge-implicit random geometric graph.

Short Description: An edge-implicit representation of a random geometric graph. TODO(Marlin): Separating the search tree from the RGG seems conceptually cleaner. Think about its implications.

Public Functions

ImplicitGraph(NameFunc nameFunc): Construct an implicit graph.

virtual ~ImplicitGraph() = default: Destruct the graph using default destruction.

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): Setup the ImplicitGraph, must be called before use. Does not take a copy of the PlannerInputStates, but checks it for starts/goals.

void reset(): Reset the graph to the state of construction.

bool hasAStart() const: Gets whether the graph contains a start or not.

bool hasAGoal() const: Gets whether the graph contains a goal or not.

VertexPtrVector::const_iterator startVerticesBeginConst() const: Returns a const-iterator to the front of the start-vertex vector.

VertexPtrVector::const_iterator startVerticesEndConst() const: Returns a const-iterator to the end of the start-vertex vector.

VertexPtrVector::const_iterator goalVerticesBeginConst() const: Returns a const-iterator to the front of the goal-vertex vector.

VertexPtrVector::const_iterator goalVerticesEndConst() const: Returns a const-iterator to the end of the goal-vertex vector.

ompl::base::Cost minCost() const: Get the minimum cost solution possible for this problem.

bool hasInformedMeasure() const: Query whether the underlying state sampler can provide an informed measure.

double getInformedMeasure(const ompl::base::Cost &cost) const: Query the underlying state sampler for the informed measure of the problem.

double distance(const VertexConstPtr &a, const VertexConstPtr &b) const: Computes the distance between two states.

double distance(const VertexConstPtrPair &vertices) const: Computes the distance between two states.

void nearestSamples(const VertexPtr &vertex, VertexPtrVector *neighbourSamples): Get the nearest unconnected samples using the appropriate “near” definition (i.e., k or r).

void getGraphAsPlannerData(ompl::base::PlannerData &data) const: Adds the graph to the given PlannerData struct.

VertexConstPtr closestVertexToGoal() const: IF BEING TRACKED, returns the closest vertex in the tree to the goal.

double smallestDistanceToGoal() const: IF BEING TRACKED, returns the how close vertices in the tree are to the goal.

unsigned int getConnectivityK() const: Get the k of this k-nearest RGG.

double getConnectivityR() const: Get the radius of this r-disc RGG.

VertexPtrVector getCopyOfSamples() const: Get a copy of all samples.

void registerSolutionCost(const ompl::base::Cost &solutionCost): Mark that a solution has been found and that the graph should be limited to the given heuristic value.

void updateStartAndGoalStates(ompl::base::PlannerInputStates &inputStates, const base::PlannerTerminationCondition &terminationCondition): Adds any new goals or starts that have appeared in the problem definition to the vector of vertices and the queue. Creates a new informed sampler if necessary.

void addNewSamples(const unsigned int &numSamples): Increase the resolution of the graph-based approximation of the continuous search domain by adding a batch of new samples.

std::pair<unsigned int, unsigned int> prune(double prunedMeasure): Prune the samples to the subproblem of the given measure. Returns the number of vertices disconnected and the number of samples removed.

void addToSamples(const VertexPtr &sample): Add an unconnected sample.

void addToSamples(const VertexPtrVector &samples): Add a vector of unconnected samples.

void removeFromSamples(const VertexPtr &sample): Remove a sample from the sample set.

void pruneSample(const VertexPtr &sample): Remove an unconnected sample.

void recycleSample(const VertexPtr &sample): Insert a sample into the set for recycled samples.

void registerAsVertex(const VertexPtr &vertex): Add a vertex to the tree, optionally moving it from the set of unconnected samples.

unsigned int removeFromVertices(const VertexPtr &sample, bool moveToFree): Remove a vertex from the tree, can optionally be allowed to move it to the set of unconnected samples if may still be useful.

std::pair<unsigned int, unsigned int> pruneVertex(const VertexPtr &vertex): Remove a vertex and mark as pruned.

void removeEdgeBetweenVertexAndParent(const VertexPtr &child, bool cascadeCostUpdates): Disconnect a vertex from its parent by removing the edges stored in itself, and its parents. Cascades cost updates if requested.

void setRewireFactor(double rewireFactor): Set the rewiring scale factor, s, such that r_rrg = s \times r_rrg*.

double getRewireFactor() const: Get the rewiring scale factor.

void setUseKNearest(bool useKNearest): Enable a k-nearest search for instead of an r-disc search.

bool getUseKNearest() const: Get whether a k-nearest search is being used.

void setJustInTimeSampling(bool useJit): Enable sampling “just-in-time”, i.e., only when necessary for a nearest-neighbour search.

bool getJustInTimeSampling() const: Get whether we’re using just-in-time sampling.

void setDropSamplesOnPrune(bool dropSamples): Set whether unconnected samples are dropped on pruning.

void setPruning(bool usePruning): Set whether samples that are provably not beneficial should be kept around.

bool getDropSamplesOnPrune() const: Get whether unconnected samples are dropped on pruning.

void setTrackApproximateSolutions(bool findApproximate): Set whether to track approximate solutions during the search.

bool getTrackApproximateSolutions() const: Get whether approximate solutions are tracked during the search.

void setAverageNumOfAllowedFailedAttemptsWhenSampling(std::size_t number): Set the average number of allowed failed attempts when sampling.

std::size_t getAverageNumOfAllowedFailedAttemptsWhenSampling() const: Get the average number of allowed failed attempts when sampling.

template<template<typename T> class NN> void setNearestNeighbors(): Set a different nearest neighbours datastructure.

unsigned int numSamples() const: The number of samples.

unsigned int numVertices() const: The number of vertices in the search tree.

unsigned int numStatesGenerated() const: The total number of states generated.

unsigned int numVerticesConnected() const: The total number of vertices added to the graph.

unsigned int numFreeStatesPruned() const: The number of states pruned.

unsigned int numVerticesDisconnected() const: The number of tree vertices disconnected.

unsigned int numNearestLookups() const: The number of nearest neighbour calls.

unsigned int numStateCollisionChecks() const: The number of state collision checks.

bool canVertexBeDisconnected(const VertexPtr &vertex) const: Returns whether the vertex can be pruned, i.e., whether it could provide a better solution given. the current graph. The check should always be g_t(v) + h^(v) >= g_t(x_g).

bool canSampleBePruned(const VertexPtr &sample) const: Returns whether the sample can be pruned, i.e., whether it could ever provide a better solution. The check should always be g^(v) + h^(v) >= g_t(x_g).

class SearchQueue

A queue of edges, sorted according to a sort key.

Short Description: A search queue holding edges ordered on a sort key, i.e., a cost triple with a lexicographical comparison. The queue is implemented as a binary heap.

Public Types

using SortKey = std::array<ompl::base::Cost, 3u>: A triplet of costs, i.e., the edge queue sorting key.

using SortKeyAndVertexPtrPair = std::pair<SortKey, VertexPtrPair>: The data stored in the edge-queue binary heap.

using EdgeComparisonFunction = std::function<bool(const SortKeyAndVertexPtrPair&, const SortKeyAndVertexPtrPair&)>: The function signature of the sorting function for the Edge Queue.

using EdgeQueue = ompl::BinaryHeap<SortKeyAndVertexPtrPair, EdgeComparisonFunction>: The underlying edge queue. Using static keys for the same reason as the Vertex Queue.

using EdgeQueueElemPtr = EdgeQueue::Element*: An element pointer into the edge queue binary heap.

using EdgeQueueElemPtrVector = std::vector<EdgeQueueElemPtr>: A vector of edge queue pointers.

Public Functions

SearchQueue(NameFunc nameFunc): Construct a search queue. It must be setup before use.

virtual ~SearchQueue() = default: Destruct the search queue using the default deconstructor.

void setup(CostHelper *costHelpPtr, ImplicitGraph *graphPtr): Setup the SearchQueue, must be called before use.

void reset(): Reset the queue to the state of construction.

void enableCascadingRewirings(bool enable): Set whether cascading of rewirings is enabled.

void insertOutgoingEdges(const VertexPtr &vertex): Update the edge queue by adding all the potential edges from the vertex to nearby states.

void insertOutgoingEdgesOfStartVertices(): Insert the outgoing edges of all start vertices.

void insertOutgoingEdgesOfInconsistentVertices(): Insert the outgoing edges of all inconsistent vertices.

void addToInconsistentSet(const VertexPtr &vertex): Add a vertex to the set of inconsistent vertices.

VertexPtrPair getFrontEdge(): Get the best edge on the queue, leaving it at the front of the edge queue.

SortKey getFrontEdgeValue(): Get the value of the best edge on the queue, leaving it at the front of the edge queue.

VertexPtrPair popFrontEdge(): Pop the best edge off the queue, removing it from the front of the edge queue in the process.

void clear(): Finish the queue if it is sorted, if not resort the queue. Finishing the queue clears all the edge containers and moves the vertex expansion token to the end. After calling finish() ON A SORTED QUEUE, isEmpty() will return true. Keeps threshold, etc.

void clearInconsistentSet(): Clear the set of inconsistent vertices.

void rebuildEdgeQueue(): Update all the sort keys of the edges in the queue and resort.

void update(const EdgeQueueElemPtr elementPtr): Update the sort key of a particular edge and its position in the queue.

void setInflationFactor(double factor): Set the cost-to-go inflation factor.

void registerSolutionCost(const ompl::base::Cost &solutionCost): Mark that a solution has been found.

void removeInEdgesConnectedToVertexFromQueue(const VertexPtr &vertex): Erase all edges in the edge queue that lead to the given vertex.

void removeOutEdgesConnectedToVertexFromQueue(const VertexPtr &vertex): Erase all edges in the edge queue that leave from the given vertex.

void removeAllEdgesConnectedToVertexFromQueue(const VertexPtr &vertex): Erase all edges in the edge queue that are connected to the given vertex.

void removeFromInconsistentSet(const VertexPtr &vertex): Remove a vertex from the set of inconsistent vertices.

double getInflationFactor() const: Get the cost-to-go inflation factor.

std::shared_ptr<const unsigned int> getSearchId() const: Allow access to the current search id.

bool canPossiblyImproveCurrentSolution(const VertexPtr &state) const: The condition used to insert vertices into the queue. Compares lowerBoundHeuristicVertex to the given threshold. Returns true if the vertex’s best cost is lower than the internally set threshold.

bool canPossiblyImproveCurrentSolution(const VertexPtrPair &edge) const: The condition used to insert edges into the queue. Compares lowerBoundHeuristicEdge to the given threshold. Returns true if the edge’s best cost is lower than the internally set threshold.

unsigned int numEdges(): Returns the number of edges in the queue. Will resort/expand the queue if necessary.

bool isEmpty(): Returns true if the queue is empty. In the case where the edge queue is empty but the vertex queue is not, this function will expand vertices until the edge queue is not empty or there are no vertices to expand.

void getEdges(VertexConstPtrPairVector *edgeQueue): Get a copy of the edge queue. This is expensive and is only meant for animations/debugging.

unsigned int numEdgesPopped() const: The number of edges popped off the queue for processing (numEdgesPopped_).

class Vertex

The vertex of the underlying graphs in gBITstar BIT*.

Short description: A class to store a state as a vertex in a (tree) graph. Allocates and frees it’s own memory on construction/destruction. Parent vertices are owned by their children as shared pointers, assuring that a parent vertex will not be deleted while the child exists. Child vertices are owned by their parents as weak pointers, assuring that the shared-pointer ownership loop is broken.
Note: Add/Remove functions should almost always update their children’s cost. The only known exception is when a series of operations are being performed and it would be beneficial to delay the update until the last operation. In this case, make sure that the last call updates the children and is on the highest ancestor that has been changed. Updates only flow downstream.

Public Functions

Vertex(ompl::base::SpaceInformationPtr spaceInformation, const CostHelper *const costHelpPtr, SearchQueue *const queuePtr, const std::shared_ptr<const unsigned int> &approximationId, bool root = false): Construct a vertex using space information, and helpers to compute various costs.

virtual ~Vertex(): Destruct a vertex.

BITstar::VertexId getId() const: The (unique) vertex ID.

ompl::base::State *state(): The state of a vertex as a pointer.

ompl::base::State const *state() const: The state of a vertex as a pointer to const.

bool isRoot() const: Returns whether the vertex is the root of the search tree.

bool hasParent() const: Returns whether this vertex has a parent.

bool isInTree() const: Get whether a vertex is in the search tree or a sample (i.e., a vertex of the RRG).

unsigned int getDepth() const: Get the depth of the vertex from the root.

VertexConstPtr getParent() const: Get a const pointer to the parent of this vertex.

VertexPtr getParent(): Get a pointer to the parent of this vertex.

bool isConsistent() const: Whether the vertex is consistent.

bool isPruned() const: Whether the vertex has been pruned.

bool isExpandedOnCurrentApproximation() const: Returns whether the vertex is expanded on current approximation.

bool isExpandedOnCurrentSearch() const: Returns whether the vertex is expaned on current search.

bool hasEverBeenExpandedAsRewiring() const: Returns whether the vertex has ever been expanded as a rewiring.

void addParent(const VertexPtr &newParent, const ompl::base::Cost &edgeInCost): Set the parent of this vertex, cannot be used to replace a previous parent. Will always update this vertex’s cost, and can update descendent costs.

void removeParent(bool updateChildCosts): Remove the parent of this vertex. Will always update this vertex’s cost, and can update the descendent costs.

bool hasChildren() const: Get whether this vertex has any children.

void getChildren(VertexConstPtrVector *children) const: Get the children of a vertex as constant pointers.

void getChildren(VertexPtrVector *children): Get the children of a vertex as mutable pointers.

void addChild(const VertexPtr &newChild): Add a child to this vertex. Does not change this vertex’s cost or those of its descendants. Child must already have this vertex listed as it’s parent.

void removeChild(const VertexPtr &oldChild): Remove a child from this vertex. Does not change this vertex’s cost or those of its descendants. Child must still have this vertex listed as its parent and it will also throw an exception if the given vertex pointer is not found.

void blacklistChild(const VertexConstPtr &vertex): Put the vertex on the blacklist of children.

void whitelistChild(const VertexConstPtr &vertex): Put the vertex on the whitelist of children.

bool isBlacklistedAsChild(const VertexConstPtr &vertex) const: Returns true if the vertex is blacklisted as a child of this vertex.

bool isWhitelistedAsChild(const VertexConstPtr &vertex) const: Returns true if the vertex is blacklisted as a child of this vertex.

void clearBlacklist(): Clears the blacklist.

void clearWhitelist(): Clears the whitelist.

ompl::base::Cost getCost() const: Get the cost-to-come of a vertex. Return infinity if the edge is disconnected.

ompl::base::Cost getEdgeInCost() const: Get the incremental cost-to-come of a vertex.

void registerExpansion(): Mark the vertex as expanded.

void registerRewiringExpansion(): Mark expansion to vertices.

void markPruned(): Mark the vertex as pruned.

void markUnpruned(): Mark the vertex as unpruned.

void insertInEdgeQueueInLookup(const SearchQueue::EdgeQueueElemPtr &inEdge): Add to the list of the edge queue entries that point in to this vertex. Will clear existing in/out lookups if they were added under a different id.

void insertInEdgeQueueOutLookup(const SearchQueue::EdgeQueueElemPtr &outEdge): Add to the list of the edge queue entries that point out of this vertex. Will clear existing in/out lookups if they were added under a different id.

void removeFromEdgeQueueInLookup(const SearchQueue::EdgeQueueElemPtr &inEdge): Remove an incoming edge queue entry by value to the member vector.

void removeFromEdgeQueueOutLookup(const SearchQueue::EdgeQueueElemPtr &outEdge): Remove an outgoing edge queue entry by value.

void removeFromEdgeQueueInLookup(const SearchQueue::EdgeQueueElemPtrVector::const_iterator &inEdge): Remove an incoming edge queue entry by iterator to the member vector.

void removeFromEdgeQueueOutLookup(const SearchQueue::EdgeQueueElemPtrVector::const_iterator &outEdge): Remove an outgoing edge queue entry by iterator to the member vector.

BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator edgeQueueInLookupConstBegin(): Get an iterator to the front of the incoming edge queue entry vector. Will clear existing in/out lookups if they were added under a different id.

BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator edgeQueueOutLookupConstBegin(): Get an iterator to the front of the outgoing edge queue entry vector. Will clear existing in/out lookups if they were added under a different id.

BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator edgeQueueInLookupConstEnd(): Get an iterator to the end of the incoming edge queue entry vector. Will clear existing in/out lookups if they were added under a different id.

BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator edgeQueueOutLookupConstEnd(): Get an iterator to the end of the outgoing edge queue entry vector. Will clear existing in/out lookups if they were added under a different id.

unsigned int edgeQueueInLookupSize(): Get the number of edge queue entries incoming to this vertex. Will clear existing in/out lookups if they were added under a different id.

unsigned int edgeQueueOutLookupSize(): Get the number of edge queue entries outgoing from this vertex. Will clear existing in/out lookups if they were added under a different id.

void clearEdgeQueueInLookup(): Clear the pointers to all of the incoming edge queue entries.

void clearEdgeQueueOutLookup(): Clear the pointers to all of the outgoing edge queue entries.