SearchQueue.cpp
79 void BITstar::SearchQueue::setup(const CostHelperPtr &costHelpPtr, const ImplicitGraphPtr &graphPtr)
150 void BITstar::SearchQueue::enqueueEdge(const VertexPtr &sourceVertex, const VertexPtr &targetVertex)
361 // The iterator to the key,value of the child-lookup map, i.e., an iterator to a pair whose second is a
376 for (auto costAndEdgePairAsQueueIter = vectorOfEdgeQueueItersToVertexAsUMapIter->second.begin();
419 // The iterator to the key, value of the parent-lookup map, i.e., an iterator to a pair whose second is
434 for (auto costAndEdgePairAsQueueIter = vectorOfEdgeQueueItersFromVertexAsUMapIter->second.begin();
477 std::pair<unsigned int, unsigned int> BITstar::SearchQueue::prune(const VertexConstPtr &goalVertexPtr)
492 // The vertex expansion queue is sorted on an estimated solution cost considering the *current* cost-to-come
494 // This means that the value of the vertices in the queue are an upper-bounding estimate of the value we
496 // Therefore, we can start our pruning at the goal vertex and iterate forward through the queue from there.
566 OMPL_INFORM("%s: Resorting %d vertices in the queue.", nameFunc_().c_str(), resortVertices_.size());
570 // Iterate over the vector and place into the unique queue indexed on *depth*. This guarantees that we
644 OMPL_INFORM("%s: Resorting the queue disconnected %d vertices from the tree and completely removed "
711 return costHelpPtr_->isCostBetterThanOrEquivalentTo(costHelpPtr_->lowerBoundHeuristicVertex(state),
721 // Can it ever be a better solution? Less-than-equal to just in case we're using an edge that is exactly
724 rval = costHelpPtr_->isCostBetterThanOrEquivalentTo(costHelpPtr_->lowerBoundHeuristicEdge(edge),
727 // If the child is connected already, we need to check if we could do better than it's current connection.
731 // Can it ever be a better path to the vertex? Less-than-equal to just in case we're using an edge that
734 rval = costHelpPtr_->isCostBetterThanOrEquivalentTo(costHelpPtr_->lowerBoundHeuristicTarget(edge),
747 // Prune the vertex if it could cannot part of a better solution in the current graph. Greater-than just in
750 return costHelpPtr_->isCostWorseThan(costHelpPtr_->currentHeuristicVertex(state), costThreshold_);
760 return costHelpPtr_->isCostWorseThanOrEquivalentTo(costHelpPtr_->lowerBoundHeuristicVertex(state),
771 // Prune the edge if it could cannot part of a better solution in the current graph. Greater-than just in
776 // If the child is connected already, we need to check if we could do better than it's current connection.
780 // Can it ever be a better path to the vertex in the current graph? Greater-than to just in case we're
815 for (QValueToVertexMMap::const_iterator vIter = vertexToExpand_; vIter != vertexQueue_.end(); ++vIter)
957 // By virtue of the vertex expansion rules, the token will always sit at the front of a group of
958 // equivalent cost vertices (that is to say, all vertices with the same cost get expanded at the same
974 for (QValueToVertexMMap::const_iterator vIter = vertexToExpand_; vIter != vertexQueue_.end(); ++vIter)
1020 // Expand a vertex if the edge queue is empty, or the vertex could place a better edge into it:
1108 // It has, which means that outgoing edges to old unconnected vertices have already been considered.
1122 // If the vertex has never been expanded into possible rewiring edges *and* either we're not delaying
1127 // If we're using an r-disc RGG, we will not have gotten the neighbour vertices yet, get them now
1135 // Iterate over the vector of connected targets and add only those who could ever provide a better
1171 void BITstar::SearchQueue::enqueueEdgeConditionally(const VertexPtr &parent, const VertexPtr &child)
1188 void BITstar::SearchQueue::processKNearest(const VertexConstPtr &vertex, VertexPtrVector *kNearSamples,
1275 // Reinsert myself, expanding if I cross the token if I am not already expanded but not removing/adding to
1296 // Iterate over the vector of iters to resort, inserting each one as a new edge, and then removing it as
1308 // Remove the old edge and its entry in the incoming lookup. No need to remove from this lookup, as
1316 std::pair<unsigned int, unsigned int> BITstar::SearchQueue::pruneBranch(const VertexPtr &branchBase)
1337 // Disconnect myself from my parent, not cascading costs as I know my children are also being disconnected:
1357 void BITstar::SearchQueue::disconnectParent(const VertexPtr &oldVertex, bool cascadeCostUpdates)
1362 throw ompl::Exception("An orphaned vertex has been passed for disconnection. Something went wrong.");
1366 // Check if my parent has already been pruned. This can occur if we're cascading vertex disconnections.
1377 void BITstar::SearchQueue::vertexInsertHelper(const VertexPtr &newVertex, bool expandIfBeforeToken,
1391 vertexIter = vertexQueue_.insert(std::make_pair(this->vertexQueueValue(newVertex), newVertex));
1399 // If the vertex queue is now of size 1, that means that this was the first vertex. Set the token to it
1410 2 The new vertex is before the token, but *not* immediately (i.e., there are vertices between it):
1413 3 The new vertex is after the token: Don't expand. It cleanly goes into the vector of vertices to
1415 Note: By shifting the token, we assure that if the new vertex is better than the best edge, it will get
1418 The cases look like this (-: expanded vertex, x: unexpanded vertex, X: token (next to expand), *: new
1430 // The vertex before the token. Remember that since we have already added the new vertex, this could be
1441 // The vertex before the token is the newly added vertex. Therefore we can just move the token up to
1471 unsigned int BITstar::SearchQueue::vertexRemoveHelper(const VertexPtr &oldVertex, bool fullyRemove)
1474 // The number of samples deleted (i.e., if this vertex is NOT recycled as a sample, this is a 1)
1476 // A copy of the vertex pointer to be removed so we can't delete it out from under ourselves (occurs when
1486 throw ompl::Exception("Cannot delete a vertex connected to a parent unless the vertex is being "
1531 void BITstar::SearchQueue::edgeInsertHelper(const VertexPtrPair &newEdge, EdgeQueueIter positionHint)
1546 edgeIter = edgeQueue_.insert(positionHint, std::make_pair(this->edgeQueueValue(newEdge), newEdge));
1558 void BITstar::SearchQueue::edgeRemoveHelper(const EdgeQueueIter &oldEdgeIter, bool rmIncomingLookup,
1639 BITstar::SearchQueue::CostDouble BITstar::SearchQueue::vertexQueueValue(const VertexPtr &vertex) const
1645 BITstar::SearchQueue::CostTriple BITstar::SearchQueue::edgeQueueValue(const VertexPtrPair &edge) const
bool edgePruneCondition(const VertexPtrPair &edge) const
The condition used to prune edge (i.e., vertex-pair) out of the queue. Compares currentHeuristicEdge ...
Definition: SearchQueue.cpp:764
void getEdges(VertexConstPtrPairVector *edgeQueue)
Get a copy of the edge queue. This is expensive and is only meant for animations/debugging.
Definition: SearchQueue.cpp:981
void markVertexUnsorted(const VertexPtr &vertex)
Mark the queue as requiring resorting downstream of the specified vertex.
Definition: SearchQueue.cpp:470
std::vector< VertexConstPtrPair > VertexConstPtrPairVector
A vector of pairs of const vertices, i.e., a vector of edges.
Definition: BITstar.h:142
bool isReset() const
Returns true if the queue is reset. This means that no edges have been expanded and the vertex expans...
Definition: SearchQueue.cpp:907
bool edgeInsertCondition(const VertexPtrPair &edge) const
The condition used to insert edges into the queue. Compares lowerBoundHeuristicEdge to the given thre...
Definition: SearchQueue.cpp:715
CostTriple frontEdgeValue()
Get the value of the best edge on the queue, leaving it at the front of the edge queue.
Definition: SearchQueue.cpp:226
bool vertexInsertCondition(const VertexPtr &state) const
The condition used to insert vertices into the queue. Compares lowerBoundHeuristicVertex to the given...
Definition: SearchQueue.cpp:702
void removeExtraEdgesTo(const VertexPtr &cVertex)
Removes edges in the edge queue that lead to the given vertex that would not be added to the queue no...
Definition: SearchQueue.cpp:354
unsigned int numVertices()
Returns the number of vertices left to expand. This has nontrivial cost, as the token must be moved t...
Definition: SearchQueue.cpp:800
std::pair< unsigned int, unsigned int > prune(const VertexConstPtr &goalVertexPtr)
Prune the vertex queue of vertices whose their lower-bound heuristic is greater then the threshold...
Definition: SearchQueue.cpp:477
void finish()
Finish the queue if it is sorted, if not resort the queue. Finishing the queue clears all the edge co...
Definition: SearchQueue.cpp:653
std::array< ompl::base::Cost, 2u > CostDouble
An alias declaration for a pair of costs, i.e., the vertex sorting key. Done as an array instead of a...
Definition: SearchQueue.h:97
STL namespace.
bool getStrictQueueOrdering() const
Get whether the queue stays strictly sorted with each rewiring.
Definition: SearchQueue.cpp:1681
void enqueueEdge(const VertexPtrPair &newEdge)
Insert an edge into the edge processing queue. The source vertex of this edge must be in the expansio...
Definition: SearchQueue.cpp:142
bool samplePruneCondition(const VertexPtr &state) const
The condition used to prune disconnected samples from the free set. Compares lowerBoundHeuristicVerte...
Definition: SearchQueue.cpp:753
SearchQueue(NameFunc nameFunc)
Construct a search queue. It must be setup before use.
Definition: SearchQueue.cpp:65
void getVertices(VertexConstPtrVector *vertexQueue)
Get a copy of the vertices in the vertex queue that are left to be expanded. This is expensive and is...
Definition: SearchQueue.cpp:963
std::shared_ptr< ImplicitGraph > ImplicitGraphPtr
An implicit graph shared pointer.
Definition: BITstar.h:148
void setDelayedRewiring(bool delayRewiring)
Delay considering rewiring edges until an initial solution is found.
Definition: SearchQueue.cpp:1686
unsigned int numEdgesFrom(const VertexPtr &pVertex)
Get the number of edges in the queue coming from a specific vertex. Will resort/expand the queue if n...
Definition: SearchQueue.cpp:859
std::shared_ptr< const Vertex > VertexConstPtr
A constant vertex shared pointer.
Definition: BITstar.h:126
unsigned int numEdgesTo(const VertexPtr &cVertex)
Get the number of edges in the queue pointing to a specific vertex. Will resort/expand the queue if n...
Definition: SearchQueue.cpp:825
void hasSolution(const ompl::base::Cost &solnCost)
Mark that a solution has been found.
Definition: SearchQueue.cpp:279
bool getDelayedRewiring() const
Get whether BIT* is delaying rewiring until a solution is found.
Definition: SearchQueue.cpp:1691
std::array< ompl::base::Cost, 3u > CostTriple
An alias declaration for a triple of costs, i.e., the edge sorting key.
Definition: SearchQueue.h:99
VertexPtr frontVertex()
Get the best vertex on the queue, leaving it at the front of the vertex queue.
Definition: SearchQueue.cpp:172
unsigned int numUnsorted() const
Return the number of vertices marked as unsorted.
Definition: SearchQueue.cpp:893
void removeEdgesFrom(const VertexPtr &pVertex)
Erase all edges in the edge queue that leave from the given vertex.
Definition: SearchQueue.cpp:322
void setup(const CostHelperPtr &costHelpPtr, const ImplicitGraphPtr &graphPtr)
Setup the SearchQueue, must be called before use.
Definition: SearchQueue.cpp:79
std::pair< VertexPtr, VertexPtr > VertexPtrPair
A pair of vertices, i.e., an edge.
Definition: BITstar.h:136
std::function< std::string()> NameFunc
A utility functor for ImplicitGraph and SearchQueue.
Definition: BITstar.h:153
bool isEmpty()
Returns true if the queue is empty. In the case where the edge queue is empty but the vertex queue is...
Definition: SearchQueue.cpp:914
unsigned int numEdgesPopped() const
The number of edges popped off the queue for processing (numEdgesPopped_).
Definition: SearchQueue.cpp:1706
void setStrictQueueOrdering(bool beStrict)
Set the queue to stay strictly sorted with each rewiring.
Definition: SearchQueue.cpp:1676
std::vector< VertexConstPtr > VertexConstPtrVector
A vector of shared const pointers.
Definition: BITstar.h:132
void unqueueVertex(const VertexPtr &oldVertex)
Erase a vertex from the vertex expansion queue. Will disconnect the vertex from its parent and remove...
Definition: SearchQueue.cpp:158
CostDouble frontVertexValue()
Get the value of the best vertex on the queue, leaving it at the front of the vertex queue...
Definition: SearchQueue.cpp:208
VertexPtrPair popFrontEdge()
Pop the best edge off the queue, removing it from the front of the edge queue in the process...
Definition: SearchQueue.cpp:268
void enqueueVertex(const VertexPtr &newVertex, bool removeFromFree)
Insert a vertex into the vertex expansion queue. Vertices remain in the vertex queue until pruned or ...
Definition: SearchQueue.cpp:134
bool isVertexExpanded(const VertexConstPtr &vertex) const
Returns whether a given vertex has been expanded or not.
Definition: SearchQueue.cpp:932
void removeEdgesTo(const VertexPtr &cVertex)
Erase all edges in the edge queue that lead to the given vertex.
Definition: SearchQueue.cpp:290
VertexPtrPair frontEdge()
Get the best edge on the queue, leaving it at the front of the edge queue.
Definition: SearchQueue.cpp:190
void reset()
Reset the queue, clearing all the edge containers and moving the vertex expansion token to the start...
Definition: SearchQueue.cpp:679
bool vertexPruneCondition(const VertexPtr &state) const
The condition used to prune vertices out of the queue. Compares currentHeuristicVertex to the given t...
Definition: SearchQueue.cpp:741
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
void resort()
Resort the queue around the marked unsorted vertices. If allowed, will simply remove any vertices tha...
Definition: SearchQueue.cpp:551
void removeExtraEdgesFrom(const VertexPtr &pVertex)
Removes edges in the edge queue that leave from the given vertex that would not be added to the queue...
Definition: SearchQueue.cpp:412
unsigned int numEdges()
Returns the number of edges in the queue. Will resort/expand the queue if necessary.
Definition: SearchQueue.cpp:790