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