SearchQueue.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2014, University of Toronto
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the University of Toronto nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Authors: Jonathan Gammell */
36 
37 // My definition:
38 #include "ompl/geometric/planners/bitstar/datastructures/SearchQueue.h"
39 
40 // For std::move
41 #include <utility>
42 // For std::lexicographical_compare and the std::*_heap functions.
43 #include <algorithm>
44 
45 // OMPL:
46 // For OMPL_INFORM et al.
47 #include "ompl/util/Console.h"
48 // For exceptions:
49 #include "ompl/util/Exception.h"
50 
51 // BIT*:
52 // The vertex class:
53 #include "ompl/geometric/planners/bitstar/datastructures/Vertex.h"
54 // The cost-helper class:
55 #include "ompl/geometric/planners/bitstar/datastructures/CostHelper.h"
56 // The implicit graph:
57 #include "ompl/geometric/planners/bitstar/datastructures/ImplicitGraph.h"
58 
59 namespace ompl
60 {
61  namespace geometric
62  {
64  // Public functions:
66  : nameFunc_(std::move(nameFunc))
67  , isSetup_(false)
68  , costHelpPtr_()
69  , graphPtr_()
70  , vertexQueue_([this](const CostDouble &lhs, const CostDouble &rhs)
71  {
72  return queueComparison(lhs, rhs);
73  }) // This tells the vertexQueue_ to use the queueComparison for sorting
74  , vertexToExpand_(vertexQueue_.begin())
75  , edgeQueue_([this](const CostTriple &lhs, const CostTriple &rhs)
76  {
77  return queueComparison(lhs, rhs);
78  }) // This tells the edgeQueue_ to use the queueComparison for sorting
79  , vertexIterLookup_()
80  , outgoingEdges_()
81  , incomingEdges_()
82  , resortVertices_()
83  , costThreshold_(std::numeric_limits<double>::infinity()) // Purposeful gibberish
84  , hasExactSolution_(false)
85  , numEdgesPopped_(0u)
86  , useStrictQueueOrdering_(false)
87  , delayRewiring_(true)
88  , pruneDuringResort_(true)
89  {
90  }
91 
92  void BITstar::SearchQueue::setup(const CostHelperPtr &costHelpPtr, const ImplicitGraphPtr &graphPtr)
93  {
94  // Store that I am setup
95  isSetup_ = true;
96 
97  // Get my copies
98  costHelpPtr_ = costHelpPtr;
99  graphPtr_ = graphPtr;
100 
101  // Set the the cost threshold to infinity to start:
102  costThreshold_ = costHelpPtr_->infiniteCost();
103  }
104 
106  {
107  // Reset everything to the state of construction OTHER than planner name and settings/parameters
108  // Keep this in the order of the constructors for easy verification:
109 
110  // Mark as cleared
111  isSetup_ = false;
112 
113  // The pointers
114  costHelpPtr_.reset();
115  graphPtr_.reset();
116 
117  // The vertex queue:
118  vertexQueue_.clear();
119  vertexToExpand_ = vertexQueue_.begin();
120 
121  // The edge queue:
122  edgeQueue_.clear();
123 
124  // The lookups:
125  vertexIterLookup_.clear();
126  outgoingEdges_.clear();
127  incomingEdges_.clear();
128 
129  // The resort vector:
130  resortVertices_.clear();
131 
132  // The cost threshold:
133  costThreshold_ = ompl::base::Cost(std::numeric_limits<double>::infinity());
134 
135  // The existence of a solution:
136  hasExactSolution_ = false;
137 
138  // Progress properties
139  numEdgesPopped_ = 0u;
140 
141  // DO NOT reset the settings:
142  // useStrictQueueOrdering_
143  // delayRewiring_
144  // pruneDuringResort_
145  }
146 
147  void BITstar::SearchQueue::enqueueVertex(const VertexPtr &newVertex, bool removeFromFree)
148  {
149  this->confirmSetup();
150 
151  // Insert the vertex:
152  this->vertexInsertHelper(newVertex, true, removeFromFree, true);
153  }
154 
156  {
157  this->confirmSetup();
158 
159  // Call my helper function:
160  this->edgeInsertHelper(newEdge, edgeQueue_.end());
161  }
162 
163  void BITstar::SearchQueue::enqueueEdge(const VertexPtr &sourceVertex, const VertexPtr &targetVertex)
164  {
165  this->confirmSetup();
166 
167  // Call my helper function:
168  this->enqueueEdge(std::make_pair(sourceVertex, targetVertex));
169  }
170 
172  {
173  this->confirmSetup();
174 
175  // Disconnect from parent if necessary, cascading cost updates:
176  if (oldVertex->hasParent() == true)
177  {
178  this->disconnectParent(oldVertex, true);
179  }
180 
181  // Remove it from vertex queue and lookup, and edge queues:
182  this->vertexRemoveHelper(oldVertex, true);
183  }
184 
186  {
187  this->confirmSetup();
188 
189 #ifdef BITSTAR_DEBUG
190  if (this->isEmpty() == true)
191  {
192  throw ompl::Exception("Attempted to access the first element in an empty SearchQueue.");
193  }
194 #endif // BITSTAR_DEBUG
195 
196  // Update the queue:
197  this->updateQueue();
198 
199  // Return the front edge
200  return vertexQueue_.begin()->second;
201  }
202 
204  {
205  this->confirmSetup();
206 
207 #ifdef BITSTAR_DEBUG
208  if (this->isEmpty() == true)
209  {
210  throw ompl::Exception("Attempted to access the first element in an empty SearchQueue.");
211  }
212 #endif // BITSTAR_DEBUG
213 
214  // Update the queue:
215  this->updateQueue();
216 
217  // Return the front edge
218  return edgeQueue_.begin()->second;
219  }
220 
222  {
223  this->confirmSetup();
224 
225 #ifdef BITSTAR_DEBUG
226  if (this->isEmpty() == true)
227  {
228  throw ompl::Exception("Attempted to access the first element in an empty SearchQueue.");
229  }
230 #endif // BITSTAR_DEBUG
231 
232  // Update the queue:
233  this->updateQueue();
234 
235  // Return the front value
236  return vertexQueue_.begin()->first;
237  }
238 
240  {
241  this->confirmSetup();
242 
243 #ifdef BITSTAR_DEBUG
244  if (this->isEmpty() == true)
245  {
246  throw ompl::Exception("Attempted to access the first element in an empty SearchQueue.");
247  }
248 #endif // BITSTAR_DEBUG
249 
250  // Update the queue:
251  this->updateQueue();
252 
253  // Return the front value
254  return edgeQueue_.begin()->first;
255  }
256 
258  {
259  this->confirmSetup();
260 
261 #ifdef BITSTAR_DEBUG
262  if (this->isEmpty() == true)
263  {
264  throw ompl::Exception("Attempted to pop an empty SearchQueue.");
265  }
266 #endif // BITSTAR_DEBUG
267 
268  // Update the queue:
269  this->updateQueue();
270 
271  // Return the front:
272  *bestEdge = edgeQueue_.begin()->second;
273 
274  // Erase the edge:
275  this->edgeRemoveHelper(edgeQueue_.begin(), true, true);
276 
277  // Increment my counter
278  ++numEdgesPopped_;
279  }
280 
282  {
283  this->confirmSetup();
284 
285  VertexPtrPair rval;
286 
287  this->popFrontEdge(&rval);
288 
289  return rval;
290  }
291 
293  {
294  this->confirmSetup();
295 
296  // Flag
297  hasExactSolution_ = true;
298 
299  // Store
300  costThreshold_ = solnCost;
301  }
302 
304  {
305  this->confirmSetup();
306 
307  if (edgeQueue_.empty() == false)
308  {
309  // Variable:
310  // The iterator to the vector of edges to the child:
311  VertexIdToEdgeQueueIterVectorUMap::iterator vidAndEdgeQueueIterVectorPairAsUMapIterToDelete;
312 
313  // Get the vector of iterators
314  vidAndEdgeQueueIterVectorPairAsUMapIterToDelete = incomingEdges_.find(cVertex->getId());
315 
316  // Make sure it was found before we start dereferencing it:
317  if (vidAndEdgeQueueIterVectorPairAsUMapIterToDelete != incomingEdges_.end())
318  {
319  // Iterate over the vector removing them from queue
320  for (auto &edgeQueueIter : vidAndEdgeQueueIterVectorPairAsUMapIterToDelete->second)
321  {
322  // Erase the edge, removing it from the *other* lookup. No need to remove from this lookup, as
323  // that's being cleared:
324  this->edgeRemoveHelper(edgeQueueIter, false, true);
325  }
326 
327  // Clear the vector:
328  vidAndEdgeQueueIterVectorPairAsUMapIterToDelete->second.clear();
329  }
330  // No else, why was this called?
331  }
332  // No else, nothing to remove_to
333  }
334 
336  {
337  this->confirmSetup();
338 
339  if (edgeQueue_.empty() == false)
340  {
341  // Variable:
342  // The iterator to the vector of edges from the parent:
343  VertexIdToEdgeQueueIterVectorUMap::iterator vidAndEdgeQueueIterVectorPairAsUMapIterToDelete;
344 
345  // Get the vector of iterators
346  vidAndEdgeQueueIterVectorPairAsUMapIterToDelete = outgoingEdges_.find(pVertex->getId());
347 
348  // Make sure it was found before we start dereferencing it:
349  if (vidAndEdgeQueueIterVectorPairAsUMapIterToDelete != outgoingEdges_.end())
350  {
351  // Iterate over the vector removing them from queue
352  for (auto &edgeQueueIter : vidAndEdgeQueueIterVectorPairAsUMapIterToDelete->second)
353  {
354  // Erase the edge, removing it from the *other* lookup. No need to remove from this lookup, as
355  // that's being cleared:
356  this->edgeRemoveHelper(edgeQueueIter, true, false);
357  }
358 
359  // Clear the vector:
360  vidAndEdgeQueueIterVectorPairAsUMapIterToDelete->second.clear();
361  }
362  // No else, why was this called?
363  }
364  // No else, nothing to remove_from
365  }
366 
368  {
369  this->confirmSetup();
370 
371  if (edgeQueue_.empty() == false)
372  {
373  // Variable:
374  // The iterator to the key,value of the child-lookup map, i.e., an iterator to a pair whose second is a
375  // vector of edges to the child (which are actually iterators to the queue):
376  VertexIdToEdgeQueueIterVectorUMap::iterator vectorOfEdgeQueueItersToVertexAsUMapIter;
377 
378  // Get my incoming edges as a vector of iterators
379  vectorOfEdgeQueueItersToVertexAsUMapIter = incomingEdges_.find(cVertex->getId());
380 
381  // Make sure it was found before we start dereferencing it:
382  if (vectorOfEdgeQueueItersToVertexAsUMapIter != incomingEdges_.end())
383  {
384  // Variable
385  // The vector of edges to delete in the vector:
386  std::vector<EdgeQueueIterVector::iterator> edgeQueueItersToDelete;
387 
388  // Iterate over the incoming edges and record those that are to be deleted
389  for (auto costAndEdgePairAsQueueIter = vectorOfEdgeQueueItersToVertexAsUMapIter->second.begin();
390  costAndEdgePairAsQueueIter != vectorOfEdgeQueueItersToVertexAsUMapIter->second.end();
391  ++costAndEdgePairAsQueueIter)
392  {
393  // Check if it would have been inserted
394  if (this->edgeInsertCondition((*costAndEdgePairAsQueueIter)->second) == false)
395  {
396  // It would not, delete
397  edgeQueueItersToDelete.push_back(costAndEdgePairAsQueueIter);
398  }
399  // No else, we're not deleting this iterator
400  }
401 
402  // Now, iterate over the vector of iterators to delete
403  for (auto &edgeQueueIter : edgeQueueItersToDelete)
404  {
405  // Remove the edge and the edge iterator from the other lookup table:
406  this->edgeRemoveHelper(*edgeQueueIter, false, true);
407 
408  // And finally erase the lookup iterator from the from lookup. If this was done first, the
409  // iterator would be invalidated for the above.
410  // Swap to the back
411  if (edgeQueueIter != (vectorOfEdgeQueueItersToVertexAsUMapIter->second.end() - 1))
412  {
413  std::swap(*edgeQueueIter, vectorOfEdgeQueueItersToVertexAsUMapIter->second.back());
414  }
415 
416  // Delete off the back
417  vectorOfEdgeQueueItersToVertexAsUMapIter->second.pop_back();
418  }
419  }
420  // No else, nothing to delete
421  }
422  // No else, nothing to prune_to
423  }
424 
426  {
427  this->confirmSetup();
428 
429  if (edgeQueue_.empty() == false)
430  {
431  // Variable:
432  // The iterator to the key, value of the parent-lookup map, i.e., an iterator to a pair whose second is
433  // a vector of edges from the child (which are actually iterators to the queue):
434  VertexIdToEdgeQueueIterVectorUMap::iterator vectorOfEdgeQueueItersFromVertexAsUMapIter;
435 
436  // Get my outgoing edges as a vector of iterators
437  vectorOfEdgeQueueItersFromVertexAsUMapIter = outgoingEdges_.find(pVertex->getId());
438 
439  // Make sure it was found before we start dereferencing it:
440  if (vectorOfEdgeQueueItersFromVertexAsUMapIter != outgoingEdges_.end())
441  {
442  // Variable
443  // The vector of edges to delete in the vector:
444  std::vector<EdgeQueueIterVector::iterator> edgeQueueItersToDelete;
445 
446  // Iterate over the incoming edges and record those that are to be deleted
447  for (auto costAndEdgePairAsQueueIter = vectorOfEdgeQueueItersFromVertexAsUMapIter->second.begin();
448  costAndEdgePairAsQueueIter != vectorOfEdgeQueueItersFromVertexAsUMapIter->second.end();
449  ++costAndEdgePairAsQueueIter)
450  {
451  // Check if it would have been inserted
452  if (this->edgeInsertCondition((*costAndEdgePairAsQueueIter)->second) == false)
453  {
454  // It would not, delete
455  edgeQueueItersToDelete.push_back(costAndEdgePairAsQueueIter);
456  }
457  // No else, we're not deleting this iterator
458  }
459 
460  // Now, iterate over the vector of iterators to delete
461  for (auto &edgeQueueIter : edgeQueueItersToDelete)
462  {
463  // Remove the edge and the edge iterator from the other lookup table:
464  this->edgeRemoveHelper(*edgeQueueIter, true, false);
465 
466  // And finally erase the lookup iterator from the from lookup. If this was done first, the
467  // iterator would be invalidated for the above.
468  // Swap to the back
469  if (edgeQueueIter != (vectorOfEdgeQueueItersFromVertexAsUMapIter->second.end() - 1))
470  {
471  std::swap(*edgeQueueIter, vectorOfEdgeQueueItersFromVertexAsUMapIter->second.back());
472  }
473 
474  // Delete off the back
475  vectorOfEdgeQueueItersFromVertexAsUMapIter->second.pop_back();
476  }
477  }
478  // No else, nothing to delete
479  }
480  // No else, nothing to prune_from
481  }
482 
484  {
485  this->confirmSetup();
486 
487  resortVertices_.push_back(vertex);
488  }
489 
490  std::pair<unsigned int, unsigned int> BITstar::SearchQueue::prune(const VertexConstPtr &goalVertexPtr)
491  {
492  this->confirmSetup();
493 
494 #ifdef BITSTAR_DEBUG
495  if (hasExactSolution_ == false)
496  {
497  throw ompl::Exception("Prune cannot be called until a solution is found");
498  }
499  if (this->isSorted() == false)
500  {
501  throw ompl::Exception("Prune cannot be called on an unsorted queue.");
502  }
503 #endif // BITSTAR_DEBUG
504 
505  // The vertex expansion queue is sorted on an estimated solution cost considering the *current* cost-to-come
506  // of the vertices, while we prune by considering the best-case cost-to-come.
507  // This means that the value of the vertices in the queue are an upper-bounding estimate of the value we
508  // will use to prune them.
509  // Therefore, we can start our pruning at the goal vertex and iterate forward through the queue from there.
510 
511  // Variables:
512  // The number of vertices and samples pruned:
513  std::pair<unsigned int, unsigned int> numPruned(0u, 0u);
514  // The iterator into the lookup helper:
515  VertexIdToVertexQueueIterUMap::iterator lookupIter;
516  // The iterator into the queue:
517  VertexQueueIter queueIter;
518 
519  // Get the iterator to the queue of the given starting point.
520  lookupIter = vertexIterLookup_.find(goalVertexPtr->getId());
521 
522 #ifdef BITSTAR_DEBUG
523  // Check that it was found
524  if (lookupIter == vertexIterLookup_.end())
525  {
526  // Complain
527  throw ompl::Exception("The provided starting point is not in the queue?");
528  }
529 #endif // BITSTAR_DEBUG
530 
531  // Get the vertex queue iterator:
532  queueIter = lookupIter->second;
533 
534  // Iterate through to the end of the queue
535  while (queueIter != vertexQueue_.end())
536  {
537  // Check if it should be pruned (value) or has lost its parent.
538  if (this->vertexPruneCondition(queueIter->second) == true)
539  {
540  // The vertex should be pruned.
541  // Variables
542  // An iter to the vertex to prune:
543  VertexQueueIter pruneIter;
544 
545  // Copy the iterator to prune:
546  pruneIter = queueIter;
547 
548  // Move the queue iterator back one so we can step to the next *valid* vertex after pruning:
549  --queueIter;
550 
551  // Prune the branch:
552  numPruned = numPruned + this->pruneBranch(pruneIter->second);
553  }
554  // No else, skip this vertex.
555 
556  // Iterate forward to the next value in the queue
557  ++queueIter;
558  }
559 
560  // Return the number of vertices and samples pruned.
561  return numPruned;
562  }
563 
565  {
566  this->confirmSetup();
567 
568  // Iterate through every vertex marked for resorting:
569  if (resortVertices_.empty() == false)
570  {
571  // Variable:
572  // The container ordered on vertex depth:
573  typedef std::unordered_map<BITstar::VertexId, VertexPtr> VertexIdToVertexPtrUMap;
574  typedef std::map<unsigned int, VertexIdToVertexPtrUMap> DepthToUMapMap;
575  DepthToUMapMap uniqueResorts;
576  // The number of vertices and samples pruned, respectively:
577  std::pair<unsigned int, unsigned int> numPruned(0u, 0u);
578 
579  OMPL_INFORM("%s: Resorting %d vertices in the queue.", nameFunc_().c_str(), resortVertices_.size());
580 
581  // Variable:
582 
583  // Iterate over the vector and place into the unique queue indexed on *depth*. This guarantees that we
584  // won't process a branch multiple times by being given different vertices down its chain
585  for (auto &vertex : resortVertices_)
586  {
587  // Add the vertex to the unordered map stored at the given depth.
588  // The [] return a reference to the existing entry, or creates a new entry:
589  uniqueResorts[vertex->getDepth()].emplace(vertex->getId(), vertex);
590  }
591 
592  // Clear the vector of vertices to resort from:
593  resortVertices_.clear();
594 
595  // Now process the vertices in order of depth.
596  for (auto &depthAndVertexMapPair : uniqueResorts)
597  {
598  for (auto &vIdAndPtrPair : depthAndVertexMapPair.second)
599  {
600  // Make sure it has not already been pruned:
601  if (vIdAndPtrPair.second->isPruned() == false)
602  {
603  // Make sure it has not already been returned to the set of samples:
604  if (vIdAndPtrPair.second->isInTree() == true)
605  {
606  // If this was a new vertex, would we *not* insert it in the queue (and do we have
607  // "permission" not to do so)?
608  if (pruneDuringResort_ == true &&
609  this->vertexInsertCondition(vIdAndPtrPair.second) == false)
610  {
611  // The vertex should just be pruned and forgotten about.
612  // Prune the branch:
613  numPruned = numPruned + this->pruneBranch(vIdAndPtrPair.second);
614  }
615  else
616  {
617  // The vertex is going to be kept.
618  // Does it have any children?
619  if (vIdAndPtrPair.second->hasChildren() == true)
620  {
621  // Variables:
622  // The vector of children:
623  VertexPtrVector resortChildren;
624 
625  // Put its children in the vector to be resorted:
626  // Get the vector of children:
627  vIdAndPtrPair.second->getChildren(&resortChildren);
628 
629  // Get a reference to the container for the children, all children are 1 level
630  // deeper than their parent.:
631  // The [] return a reference to the existing entry, or creates a new entry:
632  VertexIdToVertexPtrUMap &depthContainer =
633  uniqueResorts[vIdAndPtrPair.second->getDepth() + 1u];
634 
635  // Place the children into the container, as the container is a map, it will not
636  // allow the children to be entered twice.
637  for (auto &childPtr : resortChildren)
638  {
639  depthContainer.emplace(childPtr->getId(), childPtr);
640  }
641  }
642 
643  // Reinsert the vertex:
644  this->reinsertVertex(vIdAndPtrPair.second);
645  }
646  }
647  // No else, this vertex was a child of a vertex pruned during the resort. It has already
648  // been returned to the set of free samples by the call to pruneBranch.
649  }
650  // No else, this vertex was a child of a vertex pruned during the resort. It has already been
651  // deleted by the call to pruneBranch.
652  }
653  }
654 
655  if (numPruned.first > 0u || numPruned.second > 0u)
656  {
657  OMPL_INFORM("%s: Resorting the queue disconnected %d vertices from the tree and completely removed "
658  "%d samples.",
659  nameFunc_().c_str(), numPruned.first, numPruned.second);
660  }
661  // NO else, sshhh
662  }
663  // No else, nothing to resort
664  }
665 
667  {
668  this->confirmSetup();
669 
670  // Is there anything to resort before we're marked as finished?
671  if (resortVertices_.empty() == false)
672  {
673  OMPL_DEBUG("%s (SearchQueue): Resorting an unsorted queue instead of marking it as finished.",
674  nameFunc_().c_str());
675  this->resort();
676  }
677  else
678  {
679  // Clear the edge containers:
680  edgeQueue_.clear();
681  outgoingEdges_.clear();
682  incomingEdges_.clear();
683 
684  // Move the token to the end:
685  vertexToExpand_ = vertexQueue_.end();
686 
687  // Do NOT clear:
688  // - vertexIterLookup_ (it's still valid)
689  }
690  }
691 
693  {
694  this->confirmSetup();
695 
696 #ifdef BITSTAR_DEBUG
697  if (resortVertices_.empty() == false)
698  {
699  throw ompl::Exception("SearchQueue::reset() called with an unsorted queue.");
700  }
701 #endif // BITSTAR_DEBUG
702 
703  // Clear the edge containers:
704  edgeQueue_.clear();
705  outgoingEdges_.clear();
706  incomingEdges_.clear();
707 
708  // The resort vector:
709  resortVertices_.clear();
710 
711  // Restart the expansion queue:
712  vertexToExpand_ = vertexQueue_.begin();
713  }
714 
716  {
717  this->confirmSetup();
718 
719  // Threshold should always be g_t(x_g)
720 
721  // Can it ever be a better solution?
722  // Just in case we're using a vertex that is exactly optimally connected
723  // g^(v) + h^(v) <= g_t(x_g)?
724  return costHelpPtr_->isCostBetterThanOrEquivalentTo(costHelpPtr_->lowerBoundHeuristicVertex(state),
725  costThreshold_);
726  }
727 
729  {
730  this->confirmSetup();
731 
732  bool rval;
733 
734  // Can it ever be a better solution? Less-than-equal to just in case we're using an edge that is exactly
735  // optimally connected
736  // g^(v) + c^(v,x) + h^(x) <= g_t(x_g)?
737  rval = costHelpPtr_->isCostBetterThanOrEquivalentTo(costHelpPtr_->lowerBoundHeuristicEdge(edge),
738  costThreshold_);
739 
740  // If the child is connected already, we need to check if we could do better than it's current connection.
741  // But only if we're inserting the edge
742  if (edge.second->hasParent() == true && rval == true)
743  {
744  // Can it ever be a better path to the vertex? Less-than-equal to just in case we're using an edge that
745  // is exactly optimally connected
746  // g^(v) + c^(v,x) <= g_t(x)?
747  rval = costHelpPtr_->isCostBetterThanOrEquivalentTo(costHelpPtr_->lowerBoundHeuristicTarget(edge),
748  edge.second->getCost()); // Ever rewire?
749  }
750 
751  return rval;
752  }
753 
755  {
756  this->confirmSetup();
757 
758  // Threshold should always be g_t(x_g)
759 
760  // Prune the vertex if it could cannot part of a better solution in the current graph. Greater-than just in
761  // case we're using an edge that is exactly optimally connected.
762  // g_t(v) + h^(v) > g_t(x_g)?
763  return costHelpPtr_->isCostWorseThan(costHelpPtr_->currentHeuristicVertex(state), costThreshold_);
764  }
765 
767  {
768  this->confirmSetup();
769 
770  // Threshold should always be g_t(x_g)
771  // Prune the unconnected sample if it could never be better of a better solution.
772  // g^(v) + h^(v) >= g_t(x_g)?
773  return costHelpPtr_->isCostWorseThanOrEquivalentTo(costHelpPtr_->lowerBoundHeuristicVertex(state),
774  costThreshold_);
775  }
776 
778  {
779  this->confirmSetup();
780 
781  bool rval;
782  // Threshold should always be g_t(x_g)
783 
784  // Prune the edge if it could cannot part of a better solution in the current graph. Greater-than just in
785  // case we're using an edge that is exactly optimally connected.
786  // g_t(v) + c^(v,x) + h^(x) > g_t(x_g)?
787  rval = costHelpPtr_->isCostWorseThan(costHelpPtr_->currentHeuristicEdge(edge), costThreshold_);
788 
789  // If the child is connected already, we need to check if we could do better than it's current connection.
790  // But only if we're not pruning based on the first check
791  if (edge.second->hasParent() == true && rval == false)
792  {
793  // Can it ever be a better path to the vertex in the current graph? Greater-than to just in case we're
794  // using an edge that is exactly optimally connected
795  // g_t(v) + c^(v,x) > g_t(x)?
796  rval = costHelpPtr_->isCostWorseThan(costHelpPtr_->currentHeuristicTarget(edge),
797  edge.second->getCost()); // Currently rewire?
798  }
799 
800  return rval;
801  }
802 
804  {
805  this->confirmSetup();
806 
807  // Update the queue:
808  this->updateQueue();
809 
810  return edgeQueue_.size();
811  }
812 
814  {
815  this->confirmSetup();
816 
817  // Update the queue:
818  this->updateQueue();
819 
820  // Variables:
821  // The number of vertices left to expand:
822  unsigned int numToExpand;
823 
824  // Start at 0:
825  numToExpand = 0u;
826 
827  // Iterate until the end:
828  for (QValueToVertexMMap::const_iterator vIter = vertexToExpand_; vIter != vertexQueue_.end(); ++vIter)
829  {
830  // Increment counter:
831  ++numToExpand;
832  }
833 
834  // Return
835  return numToExpand;
836  }
837 
838  unsigned int BITstar::SearchQueue::numEdgesTo(const VertexPtr &cVertex)
839  {
840  this->confirmSetup();
841 
842  // Update the queue:
843  this->updateQueue();
844 
845  // Variables:
846  // The number of edges to, starting at 0:
847  unsigned int rval = 0u;
848 
849  // Is there anything to count?
850  if (edgeQueue_.empty() == false)
851  {
852  // Variable:
853  // The iterator to the vector of edges to the child:
854  VertexIdToEdgeQueueIterVectorUMap::const_iterator toIter;
855 
856  // Get the vector of iterators
857  toIter = incomingEdges_.find(cVertex->getId());
858 
859  // Make sure it was found before we dereferencing it:
860  if (toIter != incomingEdges_.end())
861  {
862  rval = toIter->second.size();
863  }
864  // No else, there are none.
865  }
866  // No else, there is nothing.
867 
868  // Return:
869  return rval;
870  }
871 
872  unsigned int BITstar::SearchQueue::numEdgesFrom(const VertexPtr &pVertex)
873  {
874  this->confirmSetup();
875 
876  // Update the queue:
877  this->updateQueue();
878 
879  // Variables:
880  // The number of edges to, starting at 0:
881  unsigned int rval = 0u;
882 
883  // Is there anything to count?
884  if (edgeQueue_.empty() == false)
885  {
886  // Variable:
887  // The iterator to the vector of edges from the parent:
888  VertexIdToEdgeQueueIterVectorUMap::const_iterator toIter;
889 
890  // Get the vector of iterators
891  toIter = outgoingEdges_.find(pVertex->getId());
892 
893  // Make sure it was found before we dereferencing it:
894  if (toIter != outgoingEdges_.end())
895  {
896  rval = toIter->second.size();
897  }
898  // No else, 0u.
899  }
900  // No else, there is nothing.
901 
902  // Return
903  return rval;
904  }
905 
907  {
908  this->confirmSetup();
909 
910  return resortVertices_.size();
911  }
912 
914  {
915  this->confirmSetup();
916 
917  return resortVertices_.empty();
918  }
919 
921  {
922  this->confirmSetup();
923 
924  return (vertexToExpand_ == vertexQueue_.begin() && edgeQueue_.empty());
925  }
926 
928  {
929  this->confirmSetup();
930 
931  // Update the queue:
932  this->updateQueue();
933 
934  // Expand if the edge queue is empty but the vertex queue is not:
935  while (edgeQueue_.empty() && vertexToExpand_ != vertexQueue_.end())
936  {
937  // Expand the next vertex, this pushes the token:
938  this->expandNextVertex();
939  }
940 
941  // Return whether the edge queue is empty:
942  return edgeQueue_.empty();
943  }
944 
946  {
947  this->confirmSetup();
948 
949  // Variable
950  // The vertex iterator
951  VertexIdToVertexQueueIterUMap::const_iterator lkupIter;
952 
953  // Get the lookup iterator for the provided vertex
954  lkupIter = vertexIterLookup_.find(vertex->getId());
955 
956 #ifdef BITSTAR_DEBUG
957  if (lkupIter == vertexIterLookup_.end())
958  {
959  throw ompl::Exception("Attempting to check the expansion status of a vertex not in the queue");
960  }
961 #endif // BITSTAR_DEBUG
962 
963  // Compare the value used to currently sort the vertex in the queue to the value of the token.
964  if (vertexToExpand_ == vertexQueue_.end())
965  {
966  // If the token is at the end of the queue, obviously the vertex is expanded:
967  return true;
968  }
969  else
970  {
971  // By virtue of the vertex expansion rules, the token will always sit at the front of a group of
972  // equivalent cost vertices (that is to say, all vertices with the same cost get expanded at the same
973  // time). Therefore, the vertex is expanded if it's cost is strictly better than the token.
974  return this->queueComparison(lkupIter->second->first, vertexToExpand_->first);
975  }
976  }
977 
979  {
980  this->confirmSetup();
981 
982  // Update the queue:
983  this->updateQueue();
984 
985  // Clear the given vector:
986  vertexQueue->clear();
987 
988  // Iterate until the end, pushing back:
989  for (QValueToVertexMMap::const_iterator vIter = vertexToExpand_; vIter != vertexQueue_.end(); ++vIter)
990  {
991  // Push back:
992  vertexQueue->push_back(vIter->second);
993  }
994  }
995 
997  {
998  this->confirmSetup();
999 
1000  // Update the queue:
1001  this->updateQueue();
1002 
1003  // Clear the vector
1004  edgeQueue->clear();
1005 
1006  // I don't think there's a std::copy way to do this, so just iterate
1007  for (const auto &queueElement : edgeQueue_)
1008  {
1009  edgeQueue->push_back(queueElement.second);
1010  }
1011  }
1013 
1015  // Private functions:
1016  void BITstar::SearchQueue::updateQueue()
1017  {
1018  // If we are using strict queue ordering, we must resort the queue before we update it.
1019  if (useStrictQueueOrdering_ == true)
1020  {
1021  // Resort and vertices that have been rewired.
1022  this->resort();
1023  }
1024 
1025  // Variables:
1026  // Whether to expand:
1027  bool expand;
1028 
1029  expand = true;
1030  while (expand == true)
1031  {
1032  // Check if there are any vertices to expand:
1033  if (vertexToExpand_ != vertexQueue_.end())
1034  {
1035  // Expand a vertex if the edge queue is empty, or the vertex could place a better edge into it:
1036  if (edgeQueue_.empty() == true)
1037  {
1038  // The edge queue is empty, any edge is better than this!
1039  this->expandNextVertex();
1040  }
1041  // This is isCostBetterThanOrEquivalentTo because of the second ordering criteria. The vertex
1042  // expanded could match the edge in queue on total cost, but have less cost-to-come.
1043  else if (costHelpPtr_->isCostBetterThanOrEquivalentTo(vertexToExpand_->first.at(0u),
1044  edgeQueue_.cbegin()->first.at(0u)) == true)
1045  {
1046  // The vertex *could* give a better edge than our current best edge:
1047  this->expandNextVertex();
1048  }
1049  else
1050  {
1051  // We are done expanding for now:
1052  expand = false;
1053  }
1054  }
1055  else
1056  {
1057  // There are no vertices left to expand
1058  expand = false;
1059  }
1060  }
1061  }
1062 
1063  void BITstar::SearchQueue::expandNextVertex()
1064  {
1065  // Should we expand the next vertex?
1066  if (this->vertexInsertCondition(vertexToExpand_->second) == true)
1067  {
1068  // Expand the vertex in the front:
1069  this->expandVertex(vertexToExpand_->second);
1070 
1071  // Increment the vertex token:
1072  ++vertexToExpand_;
1073  }
1074  else
1075  {
1076  // The next vertex would get pruned, so just jump to the end:
1077  vertexToExpand_ = vertexQueue_.end();
1078  }
1079  }
1080 
1081  void BITstar::SearchQueue::expandVertex(const VertexPtr &vertex)
1082  {
1083  // Should we expand this vertex?
1084  if (this->vertexInsertCondition(vertex) == true)
1085  {
1086  // Variables:
1087  // The vector of nearby samples (either within r or the k-nearest)
1088  VertexPtrVector neighbourSamples;
1089  // The vector of nearby vertices
1090  VertexPtrVector neighbourVertices;
1091 
1092  // Get the set of nearby free states
1093  graphPtr_->nearestSamples(vertex, &neighbourSamples);
1094 
1095  // If we're usjng k-nearest, we technically need to be doing to combined k-nearest.
1096  // So get the nearestVertices and do some post-processing
1097  if (graphPtr_->getUseKNearest() == true)
1098  {
1099  // Get the set of nearby vertices
1100  graphPtr_->nearestVertices(vertex, &neighbourVertices);
1101 
1102  // Post process them:
1103  this->processKNearest(vertex, &neighbourSamples, &neighbourVertices);
1104  }
1105  // No else
1106 
1107  // Add edges to unconnected targets who could ever provide a better solution:
1108  // Has the vertex been expanded into edges towards unconnected samples before?
1109  if (vertex->hasBeenExpandedToSamples() == false)
1110  {
1111  // It has not, that means none of its outgoing edges have been considered. Add them all
1112  for (auto &targetSample : neighbourSamples)
1113  {
1114  // Attempt to queue the edge.
1115  this->enqueueEdgeConditionally(vertex, targetSample);
1116  }
1117 
1118  // Mark it as expanded
1119  vertex->markExpandedToSamples();
1120  }
1121  else
1122  {
1123  // It has, which means that outgoing edges to old unconnected vertices have already been considered.
1124  // Only add those that lead to new vertices
1125  for (auto &targetSample : neighbourSamples)
1126  {
1127  // Is the target new?
1128  if (targetSample->isNew() == true)
1129  {
1130  // It is, attempt to queue the edge.
1131  this->enqueueEdgeConditionally(vertex, targetSample);
1132  }
1133  // No else, we've considered this edge before.
1134  }
1135  }
1136 
1137  // If the vertex has never been expanded into possible rewiring edges *and* either we're not delaying
1138  // rewiring or we have a solution, we add those rewiring candidates:
1139  if (vertex->hasBeenExpandedToVertices() == false &&
1140  (delayRewiring_ == false || hasExactSolution_ == true))
1141  {
1142  // If we're using an r-disc RGG, we will not have gotten the neighbour vertices yet, get them now
1143  if (graphPtr_->getUseKNearest() == false)
1144  {
1145  // Get the set of nearby vertices
1146  graphPtr_->nearestVertices(vertex, &neighbourVertices);
1147  }
1148  // No else
1149 
1150  // Iterate over the vector of connected targets and add only those who could ever provide a better
1151  // solution:
1152  for (auto &targetVertex : neighbourVertices)
1153  {
1154  // Make sure it is not the root or myself.
1155  if (targetVertex->isRoot() == false && targetVertex->getId() != vertex->getId())
1156  {
1157  // Make sure I am not already the parent
1158  if (targetVertex->getParent()->getId() != vertex->getId())
1159  {
1160  // Make sure the neighbour vertex is not already my parent:
1161  if (vertex->isRoot() == true)
1162  {
1163  // I am root, I have no parent, so attempt to queue the edge:
1164  this->enqueueEdgeConditionally(vertex, targetVertex);
1165  }
1166  else if (targetVertex->getId() != vertex->getParent()->getId())
1167  {
1168  // The neighbour is not my parent, attempt to queue the edge:
1169  this->enqueueEdgeConditionally(vertex, targetVertex);
1170  }
1171  // No else, this vertex is my parent.
1172  }
1173  // No else
1174  }
1175  // No else
1176  }
1177 
1178  // Mark the vertex as expanded into rewirings
1179  vertex->markExpandedToVertices();
1180  }
1181  // No else
1182  }
1183  // No else
1184  }
1185 
1186  void BITstar::SearchQueue::enqueueEdgeConditionally(const VertexPtr &parent, const VertexPtr &child)
1187  {
1188  // Variable:
1189  // The edge:
1190  VertexPtrPair newEdge;
1191 
1192  // Make the edge
1193  newEdge = std::make_pair(parent, child);
1194 
1195  // Should this edge be in the queue?
1196  if (this->edgeInsertCondition(newEdge) == true)
1197  {
1198  this->enqueueEdge(newEdge);
1199  }
1200  // No else, it can never provide a better solution
1201  }
1202 
1203  void BITstar::SearchQueue::processKNearest(const VertexConstPtr &vertex, VertexPtrVector *kNearSamples,
1204  VertexPtrVector *kNearVertices)
1205  {
1206  // Variables
1207  // The position in the sample vector
1208  unsigned int samplePos = 0u;
1209  // The position in the vertex vector
1210  unsigned int vertexPos = 0u;
1211 
1212  // Iterate through the first k in the combined vectors
1213  while (samplePos + vertexPos < graphPtr_->getConnectivityK() &&
1214  (samplePos < kNearSamples->size() || vertexPos < kNearVertices->size()))
1215  {
1216  // Where along are we in the relative vectors?
1217  if (samplePos < kNearSamples->size() && vertexPos >= kNearVertices->size())
1218  {
1219  // There are just samples left. Easy, move the sample token:
1220  ++samplePos;
1221  }
1222  else if (samplePos >= kNearSamples->size() && vertexPos < kNearVertices->size())
1223  {
1224  // There are just vertices left. Easy, move the vertex token:
1225  ++vertexPos;
1226  }
1227  else
1228  {
1229  // Both are left, which is closest?
1230  if (graphPtr_->distanceFunction(kNearVertices->at(vertexPos), vertex) <
1231  graphPtr_->distanceFunction(kNearSamples->at(samplePos), vertex))
1232  {
1233  // The vertex is closer than the sample, move that token:
1234  ++vertexPos;
1235  }
1236  else
1237  {
1238  // The vertex is not closer than the sample, move the sample token:
1239  ++samplePos;
1240  }
1241  }
1242  }
1243 
1244  // Now erase the extra. Resize will truncate the extras
1245  kNearSamples->resize(samplePos);
1246  kNearVertices->resize(vertexPos);
1247  }
1248 
1249  void BITstar::SearchQueue::reinsertVertex(const VertexPtr &unorderedVertex)
1250  {
1251  // Variables:
1252  // Whether the vertex is expanded.
1253  bool alreadyExpanded;
1254  // My entry in the vertex lookup:
1255  VertexIdToVertexQueueIterUMap::iterator myLookup;
1256  // The vector of edges from the vertex:
1257  VertexIdToEdgeQueueIterVectorUMap::iterator vertexAndEdgeQueueVectorVectorPairAsIter;
1258 
1259  // Get my iterator:
1260  myLookup = vertexIterLookup_.find(unorderedVertex->getId());
1261 
1262 #ifdef BITSTAR_DEBUG
1263  // Assert it was found
1264  if (myLookup == vertexIterLookup_.end())
1265  {
1266  throw ompl::Exception("Vertex to reinsert is not in the lookup. Something went wrong.");
1267  }
1268 #endif // BITSTAR_DEBUG
1269 
1270  // Test if it I am currently expanded.
1271  if (vertexToExpand_ == vertexQueue_.end())
1272  {
1273  // The token is at the end, therefore this vertex is in front of it:
1274  alreadyExpanded = true;
1275  }
1276  else if (this->queueComparison(myLookup->second->first, vertexToExpand_->first) == true)
1277  {
1278  // This vertex was entered into the queue with a cost that is in front of the current token:
1279  alreadyExpanded = true;
1280  }
1281  else
1282  {
1283  // Otherwise I have not been expanded yet.
1284  alreadyExpanded = false;
1285  }
1286 
1287  // Remove myself, not touching my lookup entries
1288  this->vertexRemoveHelper(unorderedVertex, false);
1289 
1290  // Reinsert myself, expanding if I cross the token if I am not already expanded but not removing/adding to
1291  // either NN struct
1292  this->vertexInsertHelper(unorderedVertex, alreadyExpanded == false, false, false);
1293 
1294  // Iterate over my outgoing edges and reinsert them in the queue:
1295  // Get my vector of outgoing edges
1296  vertexAndEdgeQueueVectorVectorPairAsIter = outgoingEdges_.find(unorderedVertex->getId());
1297 
1298  // Reinsert the edges:
1299  if (vertexAndEdgeQueueVectorVectorPairAsIter != outgoingEdges_.end())
1300  {
1301  // Variables
1302  // The iterators to the edge queue from this vertex
1303  EdgeQueueIterVector edgeQueueItersToResort;
1304 
1305  // Copy the iters to resort
1306  edgeQueueItersToResort = vertexAndEdgeQueueVectorVectorPairAsIter->second;
1307 
1308  // Clear the outgoing lookup
1309  vertexAndEdgeQueueVectorVectorPairAsIter->second.clear();
1310 
1311  // Iterate over the vector of iters to resort, inserting each one as a new edge, and then removing it as
1312  // an iterator from the edge queue and the incoming lookup
1313  for (auto &costAndEdgePairAsQueueIter : edgeQueueItersToResort)
1314  {
1315  // Check if the edge should be reinserted
1316  if (this->edgeInsertCondition(costAndEdgePairAsQueueIter->second) == true)
1317  {
1318  // Call helper to reinsert. Looks after lookups, hint at the location it's coming out of
1319  this->edgeInsertHelper(costAndEdgePairAsQueueIter->second, costAndEdgePairAsQueueIter);
1320  }
1321  // No else, prune.
1322 
1323  // Remove the old edge and its entry in the incoming lookup. No need to remove from this lookup, as
1324  // that's been cleared:
1325  this->edgeRemoveHelper(costAndEdgePairAsQueueIter, true, false);
1326  }
1327  }
1328  // No else, no edges from this vertex to requeue
1329  }
1330 
1331  std::pair<unsigned int, unsigned int> BITstar::SearchQueue::pruneBranch(const VertexPtr &branchBase)
1332  {
1333 #ifdef BITSTAR_DEBUG
1334  // Assert the vertex is in the tree
1335  if (branchBase->isInTree() == false)
1336  {
1337  throw ompl::Exception("Trying to prune a disconnected vertex. Something went wrong.");
1338  }
1339 #endif // BITSTAR_DEBUG
1340 
1341  // We must iterate over the children of this vertex and prune each one.
1342  // Then we must decide if this vertex (a) gets deleted or (b) placed back on the sample set.
1343  //(a) occurs if it has a lower-bound heuristic greater than the current solution
1344  //(b) occurs if it doesn't.
1345 
1346  // Variables:
1347  // The counter of vertices and samples pruned:
1348  std::pair<unsigned int, unsigned int> numPruned(1u, 0u);
1349  // The vector of my children:
1350  VertexPtrVector children;
1351 
1352  // Disconnect myself from my parent, not cascading costs as I know my children are also being disconnected:
1353  this->disconnectParent(branchBase, false);
1354 
1355  // Get the vector of children
1356  branchBase->getChildren(&children);
1357 
1358  // Remove myself from everything:
1359  numPruned.second = this->vertexRemoveHelper(branchBase, true);
1360 
1361  // Prune my children:
1362  for (auto &child : children)
1363  {
1364  // Prune my children:
1365  numPruned = numPruned + this->pruneBranch(child);
1366  }
1367 
1368  // Return the number pruned
1369  return numPruned;
1370  }
1371 
1372  void BITstar::SearchQueue::disconnectParent(const VertexPtr &oldVertex, bool cascadeCostUpdates)
1373  {
1374 #ifdef BITSTAR_DEBUG
1375  if (oldVertex->hasParent() == false)
1376  {
1377  throw ompl::Exception("An orphaned vertex has been passed for disconnection. Something went wrong.");
1378  }
1379 #endif // BITSTAR_DEBUG
1380 
1381  // Check if my parent has already been pruned. This can occur if we're cascading vertex disconnections.
1382  if (oldVertex->getParent()->isPruned() == false)
1383  {
1384  // If not, remove myself from my parent's vector of children, not updating down-stream costs
1385  oldVertex->getParent()->removeChild(oldVertex, false);
1386  }
1387 
1388  // Remove my parent link, cascading cost updates if requested:
1389  oldVertex->removeParent(cascadeCostUpdates);
1390  }
1391 
1392  void BITstar::SearchQueue::vertexInsertHelper(const VertexPtr &newVertex, bool expandIfBeforeToken,
1393  bool removeFromFree, bool addToNNStruct)
1394  {
1395  // Variable:
1396  // The iterator to the new edge in the queue:
1397  VertexQueueIter vertexIter;
1398 
1399  // Add the vertex to the graph
1400  if (addToNNStruct == true)
1401  {
1402  graphPtr_->addVertex(newVertex, removeFromFree);
1403  }
1404 
1405  // Insert into the order map, getting the iterator
1406  vertexIter = vertexQueue_.insert(std::make_pair(this->vertexQueueValue(newVertex), newVertex));
1407 
1408  // Store the iterator in the lookup. This will create if necessary and otherwise lookup
1409  vertexIterLookup_[newVertex->getId()] = vertexIter;
1410 
1411  // Check if we are in front of the token and expand if so:
1412  if (vertexQueue_.size() == 1u)
1413  {
1414  // If the vertex queue is now of size 1, that means that this was the first vertex. Set the token to it
1415  // and don't even think of expanding anything:
1416  vertexToExpand_ = vertexQueue_.begin();
1417  }
1418  else if (expandIfBeforeToken == true)
1419  {
1420  /*
1421  There are 3ish cases:
1422  1 The new vertex is immediately before the token.
1423  a The token is not at the end: Don't expand and shift the token to the new vertex.
1424  b The token is at the end: Don't expand and shift the token to the new vertex.
1425  2 The new vertex is before the token, but *not* immediately (i.e., there are vertices between it):
1426  a The token is at the end: Expand the vertex
1427  b The token is not at the end: Expand the vertex
1428  3 The new vertex is after the token: Don't expand. It cleanly goes into the vector of vertices to
1429  expand
1430  Note: By shifting the token, we assure that if the new vertex is better than the best edge, it will get
1431  expanded on the next pop.
1432 
1433  The cases look like this (-: expanded vertex, x: unexpanded vertex, X: token (next to expand), *: new
1434  vertex):
1435  We represent the token at the end with no X in the line:
1436 
1437  1a: ---*Xxx -> ---Xxxx
1438  1b: ------* -> ------X
1439  2a: ---*--- -> -------
1440  2b: --*-Xxx -> ----Xxx
1441  3: ---Xx*x -> ---Xxxx
1442  */
1443 
1444  // Variable:
1445  // The vertex before the token. Remember that since we have already added the new vertex, this could be
1446  // ourselves:
1447  VertexQueueIter preToken;
1448 
1449  // Get the vertex before the current token:
1450  preToken = vertexToExpand_;
1451  --preToken;
1452 
1453  // Check if we are immediately before: (1a & 1b)
1454  if (preToken == vertexIter)
1455  {
1456  // The vertex before the token is the newly added vertex. Therefore we can just move the token up to
1457  // the newly added vertex:
1458  vertexToExpand_ = vertexIter;
1459  }
1460  else
1461  {
1462  // We are not immediately before the token.
1463 
1464  // Check if the token is at the end (2a)
1465  if (vertexToExpand_ == vertexQueue_.end())
1466  {
1467  // It is. We've expanded the whole queue, and the new vertex isn't at the end of the queue.
1468  // Expand!
1469  this->expandVertex(newVertex);
1470  }
1471  else
1472  {
1473  // The token is not at the end. That means we can safely dereference it:
1474  // Are we in front of it (2b)?
1475  if (this->queueComparison(this->vertexQueueValue(newVertex), vertexToExpand_->first) == true)
1476  {
1477  // We're before it, so expand it:
1478  this->expandVertex(newVertex);
1479  }
1480  // No else, the vertex is behind the current token (3) and will get expanded as necessary.
1481  }
1482  }
1483  }
1484  }
1485 
1486  unsigned int BITstar::SearchQueue::vertexRemoveHelper(const VertexPtr &oldVertex, bool fullyRemove)
1487  {
1488  // Variable
1489  // The number of samples deleted (i.e., if this vertex is NOT recycled as a sample, this is a 1)
1490  unsigned int deleted = 0u;
1491  // A copy of the vertex pointer to be removed so we can't delete it out from under ourselves (occurs when
1492  // this function is given an element of the maintained set as the argument)
1493  VertexPtr vertexToDelete(oldVertex);
1494  // The iterator into the lookup:
1495  VertexIdToVertexQueueIterUMap::iterator lookupIter = vertexIterLookup_.find(vertexToDelete->getId());
1496 
1497 #ifdef BITSTAR_DEBUG
1498  // Check that the vertex is not connected to a parent:
1499  if (vertexToDelete->hasParent() == true && fullyRemove == true)
1500  {
1501  throw ompl::Exception("Cannot delete a vertex connected to a parent unless the vertex is being "
1502  "immediately reinserted, in which case fullyRemove should be false.");
1503  }
1504  // Assert there is something to delete:
1505  if (vertexQueue_.empty() == true)
1506  {
1507  std::cout << std::endl
1508  << "vId: " << vertexToDelete->getId() << std::endl;
1509  throw ompl::Exception("Removing a nonexistent vertex. Something went wrong.");
1510  }
1511  // Assert that it was found
1512  if (lookupIter == vertexIterLookup_.end())
1513  {
1514  std::cout << std::endl
1515  << "vId: " << vertexToDelete->getId() << std::endl;
1516  throw ompl::Exception("Deleted vertex is not found in lookup. Something went wrong.");
1517  }
1518 #endif // BITSTAR_DEBUG
1519 
1520  // Check if we need to move the expansion token:
1521  if (lookupIter->second == vertexToExpand_)
1522  {
1523  // It is the token, move it to the next:
1524  ++vertexToExpand_;
1525  }
1526  // No else, not the token.
1527 
1528  // Remove myself from the vertex queue:
1529  vertexQueue_.erase(lookupIter->second);
1530 
1531  // Remove from lookups map as requested
1532  if (fullyRemove == true)
1533  {
1534  vertexIterLookup_.erase(lookupIter);
1535  this->removeEdgesFrom(vertexToDelete);
1536  this->removeEdgesTo(vertexToDelete);
1537 
1538  // Remove myself from the set of connected vertices, this will recycle if necessary.
1539  deleted = graphPtr_->removeVertex(vertexToDelete, true);
1540  }
1541 
1542  // Return if the sample was deleted:
1543  return deleted;
1544  }
1545 
1546  void BITstar::SearchQueue::edgeInsertHelper(const VertexPtrPair &newEdge, EdgeQueueIter positionHint)
1547  {
1548  // Variable:
1549  // The iterator to the new edge in the queue:
1550  EdgeQueueIter edgeIter;
1551 
1552  // Insert into the edge queue, getting the iter
1553  if (positionHint == edgeQueue_.end())
1554  {
1555  // No hint, insert:
1556  edgeIter = edgeQueue_.insert(std::make_pair(this->edgeQueueValue(newEdge), newEdge));
1557  }
1558  else
1559  {
1560  // Insert with hint:
1561  edgeIter = edgeQueue_.insert(positionHint, std::make_pair(this->edgeQueueValue(newEdge), newEdge));
1562  }
1563 
1564  // Push the newly created edge back on the vector of edges from the parent.
1565  // The [] return an reference to the existing entry, or create a new entry:
1566  outgoingEdges_[newEdge.first->getId()].push_back(edgeIter);
1567 
1568  // Push the newly created edge back on the vector of edges from the child.
1569  // The [] return an reference to the existing entry, or create a new entry:
1570  incomingEdges_[newEdge.second->getId()].push_back(edgeIter);
1571  }
1572 
1573  void BITstar::SearchQueue::edgeRemoveHelper(const EdgeQueueIter &oldEdgeIter, bool rmIncomingLookup,
1574  bool rmOutgoingLookup)
1575  {
1576  // Erase the lookup tables:
1577  if (rmIncomingLookup == true)
1578  {
1579  // Erase the entry in the outgoing lookup table:
1580  this->rmEdgeLookupHelper(incomingEdges_, oldEdgeIter->second.second->getId(), oldEdgeIter);
1581  }
1582  // No else
1583 
1584  if (rmOutgoingLookup == true)
1585  {
1586  // Erase the entry in the ingoing lookup table:
1587  this->rmEdgeLookupHelper(outgoingEdges_, oldEdgeIter->second.first->getId(), oldEdgeIter);
1588  }
1589  // No else
1590 
1591  // Finally erase from the queue:
1592  edgeQueue_.erase(oldEdgeIter);
1593  }
1594 
1595  void BITstar::SearchQueue::rmEdgeLookupHelper(VertexIdToEdgeQueueIterVectorUMap &lookup,
1596  const BITstar::VertexId &idx, const EdgeQueueIter &mmapIterToRm)
1597  {
1598  // Variable:
1599  // An iterator to the vertex,vector pair in the lookup
1600  VertexIdToEdgeQueueIterVectorUMap::iterator iterToVertexVectorPair;
1601  // Whether I've found the mmapIterToRm in my vector:
1602  bool found = false;
1603  // The iterator to the mmapIterToRm in my vector:
1604  EdgeQueueIterVector::iterator iterToVector;
1605 
1606  // Get the vector in the lookup for the given index:
1607  iterToVertexVectorPair = lookup.find(idx);
1608 
1609 #ifdef BITSTAR_DEBUG
1610  // Make sure it was actually found before derefencing it:
1611  if (iterToVertexVectorPair == lookup.end())
1612  {
1613  throw ompl::Exception("Indexing vertex not found in lookup hash.");
1614  }
1615 #endif // BITSTAR_DEBUG
1616 
1617  // Start at the front:
1618  iterToVector = iterToVertexVectorPair->second.begin();
1619 
1620  // Iterate through the vector and find mmapIterToRm
1621  while (found == false && iterToVector != iterToVertexVectorPair->second.end())
1622  {
1623  // Compare the value in the vector to the target:
1624  if (*iterToVector == mmapIterToRm)
1625  {
1626  // Mark as found:
1627  found = true;
1628 
1629  // Swap it to the back
1630  if (iterToVector != (iterToVertexVectorPair->second.end() - 1))
1631  {
1632  std::swap(*iterToVector, iterToVertexVectorPair->second.back());
1633  }
1634 
1635  // Delete it off the back
1636  iterToVertexVectorPair->second.pop_back();
1637  }
1638  else
1639  {
1640  // Increment the iterator:
1641  ++iterToVector;
1642  }
1643  }
1644 
1645 #ifdef BITSTAR_DEBUG
1646  // Make sure it was actually found:
1647  if (found == false)
1648  {
1649  throw ompl::Exception("Edge iterator not found under given index in lookup hash.");
1650  }
1651 #endif // BITSTAR_DEBUG
1652  }
1653 
1654  BITstar::SearchQueue::CostDouble BITstar::SearchQueue::vertexQueueValue(const VertexPtr &vertex) const
1655  {
1656  // Construct and return an array
1657  return CostDouble{costHelpPtr_->currentHeuristicVertex(vertex), vertex->getCost()};
1658  }
1659 
1660  BITstar::SearchQueue::CostTriple BITstar::SearchQueue::edgeQueueValue(const VertexPtrPair &edge) const
1661  {
1662  // Construct and return an array
1663  return CostTriple{costHelpPtr_->currentHeuristicEdge(edge), costHelpPtr_->currentHeuristicTarget(edge),
1664  edge.first->getCost()};
1665  }
1666 
1667  template <std::size_t SIZE>
1668  bool BITstar::SearchQueue::queueComparison(const std::array<ompl::base::Cost, SIZE> &lhs,
1669  const std::array<ompl::base::Cost, SIZE> &rhs) const
1670  {
1671  return std::lexicographical_compare(lhs.cbegin(), lhs.cend(), rhs.cbegin(), rhs.cend(),
1672  [this](const ompl::base::Cost &a, const ompl::base::Cost &b)
1673  {
1674  return costHelpPtr_->isCostBetterThan(a, b);
1675  });
1676  }
1677 
1678  void BITstar::SearchQueue::confirmSetup() const
1679  {
1680 #ifdef BITSTAR_DEBUG
1681  if (isSetup_ == false)
1682  {
1683  throw ompl::Exception("BITstar::SearchQueue was used before it was setup.");
1684  }
1685 #endif // BITSTAR_DEBUG
1686  }
1688 
1690  // Boring sets/gets (Public):
1692  {
1693  useStrictQueueOrdering_ = beStrict;
1694  }
1695 
1697  {
1698  return useStrictQueueOrdering_;
1699  }
1700 
1702  {
1703  delayRewiring_ = delayRewiring;
1704  }
1705 
1707  {
1708  return delayRewiring_;
1709  }
1710 
1712  {
1713  pruneDuringResort_ = prune;
1714  }
1715 
1717  {
1718  return pruneDuringResort_;
1719  }
1720 
1722  {
1723  return numEdgesPopped_;
1724  }
1726  } // geometric
1727 } // ompl
bool edgePruneCondition(const VertexPtrPair &edge) const
The condition used to prune edge (i.e., vertex-pair) out of the queue. Compares currentHeuristicEdge ...
void getEdges(VertexConstPtrPairVector *edgeQueue)
Get a copy of the edge queue. This is expensive and is only meant for animations/debugging.
bool getPruneDuringResort() const
Prune during resorts.
void markVertexUnsorted(const VertexPtr &vertex)
Mark the queue as requiring resorting downstream of the specified vertex.
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...
bool edgeInsertCondition(const VertexPtrPair &edge) const
The condition used to insert edges into the queue. Compares lowerBoundHeuristicEdge to the given thre...
CostTriple frontEdgeValue()
Get the value of the best edge on the queue, leaving it at the front of the edge queue.
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...
unsigned int numVertices()
Returns the number of vertices left to expand. This has nontrivial cost, as the token must be moved t...
void finish()
Finish the queue if it is sorted, if not resort the queue. Finishing the queue clears all the edge co...
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.
void enqueueEdge(const VertexPtrPair &newEdge)
Insert an edge into the edge processing queue. The source vertex of this edge must be in the expansio...
bool isSorted() const
Return whether the queue is still sorted.
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...
bool vertexInsertCondition(const VertexPtr &vertex) const
The condition used to insert vertices into the queue. Compares lowerBoundHeuristicVertex to the given...
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.
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...
std::shared_ptr< const Vertex > VertexConstPtr
A constant vertex shared pointer.
Definition: BITstar.h:126
bool vertexPruneCondition(const VertexPtr &vertex) const
The condition used to prune vertices out of the queue. Compares currentHeuristicVertex to the given t...
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...
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
void hasSolution(const ompl::base::Cost &solnCost)
Mark that a solution has been found.
bool getDelayedRewiring() const
Get whether BIT* is delaying rewiring until a solution is found.
std::pair< unsigned int, unsigned int > prune(const VertexConstPtr &pruneStartPtr)
Prune the vertex queue of vertices whose their lower-bound heuristic is greater then the threshold...
bool samplePruneCondition(const VertexPtr &vertex) const
The condition used to prune disconnected samples from the free set. Compares lowerBoundHeuristicVerte...
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.
unsigned int numUnsorted() const
Return the number of vertices marked as unsorted.
void removeEdgesFrom(const VertexPtr &pVertex)
Erase all edges in the edge queue that leave from the given vertex.
void setup(const CostHelperPtr &costHelpPtr, const ImplicitGraphPtr &graphPtr)
Setup the SearchQueue, must be called before use.
Definition: SearchQueue.cpp:92
std::pair< VertexPtr, VertexPtr > VertexPtrPair
A pair of vertices, i.e., an edge.
Definition: BITstar.h:136
The exception type for ompl.
Definition: Exception.h:46
std::function< std::string()> NameFunc
A utility functor for ImplicitGraph and SearchQueue.
Definition: BITstar.h:153
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
bool isEmpty()
Returns true if the queue is empty. In the case where the edge queue is empty but the vertex queue is...
unsigned int numEdgesPopped() const
The number of edges popped off the queue for processing (numEdgesPopped_).
std::vector< VertexPtr > VertexPtrVector
A vector of shared pointers.
Definition: BITstar.h:130
void setStrictQueueOrdering(bool beStrict)
Set the queue to stay strictly sorted with each rewiring.
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...
CostDouble frontVertexValue()
Get the value of the best vertex on the queue, leaving it at the front of the vertex queue...
VertexPtrPair popFrontEdge()
Pop the best edge off the queue, removing it from the front of the edge queue in the process...
void enqueueVertex(const VertexPtr &newVertex, bool removeFromFree)
Insert a vertex into the vertex expansion queue. Vertices remain in the vertex queue until pruned or ...
bool isVertexExpanded(const VertexConstPtr &vertex) const
Returns whether a given vertex has been expanded or not.
std::shared_ptr< CostHelper > CostHelperPtr
A cost helper shared pointer.
Definition: BITstar.h:146
void removeEdgesTo(const VertexPtr &cVertex)
Erase all edges in the edge queue that lead to the given vertex.
VertexPtrPair frontEdge()
Get the best edge on the queue, leaving it at the front of the edge queue.
void reset()
Reset the queue, clearing all the edge containers and moving the vertex expansion token to the start...
std::shared_ptr< Vertex > VertexPtr
A vertex shared pointer.
Definition: BITstar.h:121
void clear()
Clear the queue to the state of construction.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
void setPruneDuringResort(bool prune)
Prune during resorts.
unsigned int VertexId
The vertex id type.
Definition: BITstar.h:134
void resort()
Resort the queue around the marked unsorted vertices. If allowed, will simply remove any vertices tha...
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...
unsigned int numEdges()
Returns the number of edges in the queue. Will resort/expand the queue if necessary.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68