Class BITstar::ImplicitGraph

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).