Class BITstar::ImplicitGraph
Defined in File ImplicitGraph.h
Nested Relationships
This class is a nested type of Class BITstar.
Class Documentation
-
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).