ImplicitGraph.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, Marlin Strub */
36 
37 // My definition:
38 #include "ompl/geometric/planners/informedtrees/bitstar/ImplicitGraph.h"
39 
40 // STL/Boost:
41 // For std::move
42 #include <utility>
43 // For smart pointers
44 #include <memory>
45 // For, you know, math
46 #include <cmath>
47 // For boost math constants
48 #include <boost/math/constants/constants.hpp>
49 
50 // OMPL:
51 // For OMPL_INFORM et al.
52 #include "ompl/util/Console.h"
53 // For exceptions:
54 #include "ompl/util/Exception.h"
55 // For SelfConfig
56 #include "ompl/tools/config/SelfConfig.h"
57 // For RNG
58 #include "ompl/util/RandomNumbers.h"
59 // For geometric equations like unitNBallMeasure
60 #include "ompl/util/GeometricEquations.h"
61 
62 // BIT*:
63 // A collection of common helper functions
64 #include "ompl/geometric/planners/informedtrees/bitstar/HelperFunctions.h"
65 // The vertex class:
66 #include "ompl/geometric/planners/informedtrees/bitstar/Vertex.h"
67 // The cost-helper class:
68 #include "ompl/geometric/planners/informedtrees/bitstar/CostHelper.h"
69 // The search queue class
70 #include "ompl/geometric/planners/informedtrees/bitstar/SearchQueue.h"
71 
72 // Debug macros
73 #ifdef BITSTAR_DEBUG
74 
75 #define ASSERT_SETUP this->assertSetup();
76 #else
77 #define ASSERT_SETUP
78 #endif // BITSTAR_DEBUG
79 
80 namespace ompl
81 {
82  namespace geometric
83  {
85  // Public functions:
87  : nameFunc_(std::move(nameFunc)), approximationId_(std::make_shared<unsigned int>(1u))
88  {
89  }
90 
92  const ompl::base::ProblemDefinitionPtr &problemDefinition,
93  CostHelper *costHelper, SearchQueue *searchQueue,
94  const ompl::base::Planner *plannerPtr,
95  ompl::base::PlannerInputStates &inputStates)
96  {
97  // Store that I am setup so that any debug-level tests will pass. This requires assuring that this function
98  // is ordered properly.
99  isSetup_ = true;
100 
101  // Store arguments
102  spaceInformation_ = spaceInformation;
103  problemDefinition_ = problemDefinition;
104  costHelpPtr_ = costHelper;
105  queuePtr_ = searchQueue;
106 
107  // Configure the nearest-neighbour constructs.
108  // Only allocate if they are empty (as they can be set to a specific version by a call to
109  // setNearestNeighbors)
110  if (!static_cast<bool>(samples_))
111  {
112  samples_.reset(ompl::tools::SelfConfig::getDefaultNearestNeighbors<VertexPtr>(plannerPtr));
113  }
114  // No else, already allocated (by a call to setNearestNeighbors())
115 
116  // Configure:
118  [this](const VertexConstPtr &a, const VertexConstPtr &b) { return distance(a, b); });
119  samples_->setDistanceFunction(distanceFunction);
120 
121  // Set the min, max and sampled cost to the proper objective-based values:
122  minCost_ = costHelpPtr_->infiniteCost();
123  solutionCost_ = costHelpPtr_->infiniteCost();
124  sampledCost_ = costHelpPtr_->infiniteCost();
125 
126  // Add any start and goals vertices that exist to the queue, but do NOT wait for any more goals:
127  this->updateStartAndGoalStates(inputStates, ompl::base::plannerAlwaysTerminatingCondition());
128 
129  // Get the measure of the problem
130  approximationMeasure_ = spaceInformation_->getSpaceMeasure();
131 
132  // Does the problem have finite boundaries?
133  if (!std::isfinite(approximationMeasure_))
134  {
135  // It does not, so let's estimate a measure of the planning problem.
136  // A not horrible place to start would be hypercube proportional to the distance between the start and
137  // goal. It's not *great*, but at least it sort of captures the order-of-magnitude of the problem.
138 
139  // First, some asserts.
140  // Check that JIT sampling is on, which is required for infinite problems
141  if (!useJustInTimeSampling_)
142  {
143  throw ompl::Exception("For unbounded planning problems, just-in-time sampling must be enabled "
144  "before calling setup.");
145  }
146  // No else
147 
148  // Check that we have a start and goal
149  if (startVertices_.empty() || goalVertices_.empty())
150  {
151  throw ompl::Exception("For unbounded planning problems, at least one start and one goal must exist "
152  "before calling setup.");
153  }
154  // No else
155 
156  // Variables
157  // The maximum distance between start and goal:
158  double maxDist = 0.0;
159  // The scale on the maximum distance, i.e. the width of the hypercube is equal to this value times the
160  // distance between start and goal.
161  // This number is completely made up.
162  double distScale = 2.0;
163 
164  // Find the max distance
165  for (const auto &startVertex : startVertices_)
166  {
167  for (const auto &goalVertex : goalVertices_)
168  {
169  maxDist =
170  std::max(maxDist, spaceInformation_->distance(startVertex->state(), goalVertex->state()));
171  }
172  }
173 
174  // Calculate an estimate of the problem measure by (hyper)cubing the max distance
175  approximationMeasure_ = std::pow(distScale * maxDist, spaceInformation_->getStateDimension());
176  }
177  // No else, finite problem dimension
178 
179  // Finally initialize the nearestNeighbour terms:
180  // Calculate the k-nearest constant
181  k_rgg_ = this->calculateMinimumRggK();
182 
183  // Make the initial k all vertices:
184  k_ = startVertices_.size() + goalVertices_.size();
185 
186  // Make the initial r infinity
187  r_ = std::numeric_limits<double>::infinity();
188  }
189 
191  {
192  // Reset everything to the state of construction OTHER than planner name and settings/parameters
193  // Keep this in the order of the constructors for easy verification:
194 
195  // Mark as cleared
196  isSetup_ = false;
197 
198  // Pointers given at setup
199  spaceInformation_.reset();
200  problemDefinition_.reset();
201  costHelpPtr_ = nullptr;
202  queuePtr_ = nullptr;
203 
204  // Sampling
205  rng_ = ompl::RNG();
206  sampler_.reset();
207 
208  // Containers
209  startVertices_.clear();
210  goalVertices_.clear();
211  prunedStartVertices_.clear();
212  prunedGoalVertices_.clear();
213  newSamples_.clear();
214  recycledSamples_.clear();
215 
216  // The set of samples
217  if (static_cast<bool>(samples_))
218  {
219  samples_->clear();
220  samples_.reset();
221  }
222  // No else, not allocated
223 
224  // The various calculations and tracked values, same as in the header
225  numNewSamplesInCurrentBatch_ = 0u;
226  numUniformStates_ = 0u;
227  r_ = 0.0;
228  k_rgg_ = 0.0; // This is a double for better rounding later
229  k_ = 0u;
230 
231  approximationMeasure_ = 0.0;
232  minCost_ = ompl::base::Cost(std::numeric_limits<double>::infinity());
233  solutionCost_ = ompl::base::Cost(std::numeric_limits<double>::infinity());
234  sampledCost_ = ompl::base::Cost(std::numeric_limits<double>::infinity());
235  hasExactSolution_ = false;
236  closestVertexToGoal_.reset();
237  closestDistanceToGoal_ = std::numeric_limits<double>::infinity();
238 
239  // The planner property trackers:
240  numSamples_ = 0u;
241  numVertices_ = 0u;
242  numFreeStatesPruned_ = 0u;
243  numVerticesDisconnected_ = 0u;
244  numNearestNeighbours_ = 0u;
245  numStateCollisionChecks_ = 0u;
246 
247  // The approximation id.
248  *approximationId_ = 1u;
249 
250  // The various convenience pointers:
251  // DO NOT reset the parameters:
252  // rewireFactor_
253  // useKNearest_
254  // useJustInTimeSampling_
255  // dropSamplesOnPrune_
256  // findApprox_
257  }
258 
260  {
261  ASSERT_SETUP
262 
263 #ifdef BITSTAR_DEBUG
264  if (!static_cast<bool>(a->state()))
265  {
266  throw ompl::Exception("a->state is unallocated");
267  }
268  if (!static_cast<bool>(b->state()))
269  {
270  throw ompl::Exception("b->state is unallocated");
271  }
272 #endif // BITSTAR_DEBUG
273 
274  // Using RRTstar as an example, this order gives us the distance FROM the queried state TO the other
275  // neighbours in the structure.
276  // The distance function between two states
277  return spaceInformation_->distance(b->state(), a->state());
278  }
279 
281  {
282  return this->distance(vertices.first, vertices.second);
283  }
284 
285  void BITstar::ImplicitGraph::nearestSamples(const VertexPtr &vertex, VertexPtrVector *neighbourSamples)
286  {
287  ASSERT_SETUP
288 
289  // Make sure sampling has happened first.
290  this->updateSamples(vertex);
291 
292  // Keep track of how many times we've requested nearest neighbours.
293  ++numNearestNeighbours_;
294 
295  if (useKNearest_)
296  {
297  samples_->nearestK(vertex, k_, *neighbourSamples);
298  }
299  else
300  {
301  samples_->nearestR(vertex, r_, *neighbourSamples);
302  }
303  }
304 
306  {
307  ASSERT_SETUP
308 
309  // base::PlannerDataVertex takes a raw pointer to a state. I want to guarantee, that the state lives as long
310  // as the program lives.
311  static std::set<std::shared_ptr<Vertex>,
312  std::function<bool(const std::shared_ptr<Vertex> &, const std::shared_ptr<Vertex> &)>>
313  liveStates([](const auto &lhs, const auto &rhs) { return lhs->getId() < rhs->getId(); });
314 
315  // Add samples
316  if (static_cast<bool>(samples_))
317  {
318  // Get the samples as a vector.
319  VertexPtrVector samples;
320  samples_->list(samples);
321 
322  // Iterate through the samples.
323  for (const auto &sample : samples)
324  {
325  // Make sure the sample is not destructed before BIT* is.
326  liveStates.insert(sample);
327 
328  // Add sample.
329  if (!sample->isRoot())
330  {
331  data.addVertex(ompl::base::PlannerDataVertex(sample->state(), sample->getId()));
332 
333  // Add incoming edge.
334  if (sample->hasParent())
335  {
336  data.addEdge(ompl::base::PlannerDataVertex(sample->getParent()->state(),
337  sample->getParent()->getId()),
338  ompl::base::PlannerDataVertex(sample->state(), sample->getId()));
339  }
340  }
341  else
342  {
343  data.addStartVertex(ompl::base::PlannerDataVertex(sample->state(), sample->getId()));
344  }
345  }
346  }
347  // No else.
348  }
349 
351  {
352  ASSERT_SETUP
353 
354  // We have a solution!
355  hasExactSolution_ = true;
356 
357  // Store it's cost as the maximum we'd ever want to sample
358  solutionCost_ = solutionCost;
359 
360  // Clear the approximate solution
361  closestDistanceToGoal_ = std::numeric_limits<double>::infinity();
362  closestVertexToGoal_.reset();
363  }
364 
366  ompl::base::PlannerInputStates &inputStates, const base::PlannerTerminationCondition &terminationCondition)
367  {
368  ASSERT_SETUP
369 
370  // Variable
371  // Whether we've added a start or goal:
372  bool addedGoal = false;
373  bool addedStart = false;
374  /*
375  Add the new starts and goals to the vectors of said vertices. Do goals first, as they are only added as
376  samples. We do this as nested conditions so we always call nextGoal(ptc) at least once (regardless of
377  whether there are moreGoalStates or not) in case we have been given a non trivial PTC that wants us to wait,
378  but do *not* call it again if there are no more goals (as in the nontrivial PTC case, doing so would cause
379  us to wait out the ptc and never try to solve anything)
380  */
381  do
382  {
383  // Variable
384  // A new goal pointer, if there are none, it will be a nullptr.
385  // We will wait for the duration of PTC for a new goal to appear.
386  const ompl::base::State *newGoal = inputStates.nextGoal(terminationCondition);
387 
388  // Check if it's valid
389  if (static_cast<bool>(newGoal))
390  {
391  // Allocate the vertex pointer
392  goalVertices_.push_back(
393  std::make_shared<Vertex>(spaceInformation_, costHelpPtr_, queuePtr_, approximationId_));
394 
395  // Copy the value into the state
396  spaceInformation_->copyState(goalVertices_.back()->state(), newGoal);
397 
398  // And add this goal to the set of samples:
399  this->addToSamples(goalVertices_.back());
400 
401  // Mark that we've added:
402  addedGoal = true;
403  }
404  // No else, there was no goal.
405  } while (inputStates.haveMoreGoalStates());
406 
407  /*
408  And then do the same for starts. We do this last as the starts are added to the queue, which uses a
409  cost-to-go heuristic in it's ordering, and for that we want all the goals updated. As there is no way to
410  wait for new *start* states, this loop can be cleaner There is no need to rebuild the queue when we add
411  start vertices, as the queue is ordered on current cost-to-come, and adding a start doesn't change that.
412  */
413  while (inputStates.haveMoreStartStates())
414  {
415  // Variable
416  // A new start pointer, if PlannerInputStates finds that it is invalid we will get a nullptr.
417  const ompl::base::State *newStart = inputStates.nextStart();
418 
419  // Check if it's valid
420  if (static_cast<bool>(newStart))
421  {
422  // Allocate the vertex pointer:
423  startVertices_.push_back(
424  std::make_shared<Vertex>(spaceInformation_, costHelpPtr_, queuePtr_, approximationId_, true));
425 
426  // Copy the value into the state.
427  spaceInformation_->copyState(startVertices_.back()->state(), newStart);
428 
429  // Add the vertex to the set of vertices.
430  this->addToSamples(startVertices_.back());
431  this->registerAsVertex(startVertices_.back());
432 
433  // Mark that we've added:
434  addedStart = true;
435  }
436  // No else, there was no start.
437  }
438 
439  // Now, if we added a new start and have previously pruned goals, we may want to readd them.
440  if (addedStart && !prunedGoalVertices_.empty())
441  {
442  // Variable
443  // An iterator to the vector of pruned goals
444  auto prunedGoalIter = prunedGoalVertices_.begin();
445  // The end point of the vector to consider. We will delete by swapping elements to the end, moving this
446  // iterator towards the start, and then erasing once at the end.
447  auto prunedGoalEnd = prunedGoalVertices_.end();
448 
449  // Consider each one
450  while (prunedGoalIter != prunedGoalEnd)
451  {
452  // Mark as unpruned
453  (*prunedGoalIter)->markUnpruned();
454 
455  // Check if it should be readded (i.e., would it be pruned *now*?)
456  if (this->canVertexBeDisconnected(*prunedGoalIter))
457  {
458  // It would be pruned, so remark as pruned
459  (*prunedGoalIter)->markPruned();
460 
461  // and move onto the next:
462  ++prunedGoalIter;
463  }
464  else
465  {
466  // It would not be pruned now, so readd it!
467  // Add back to the vector:
468  goalVertices_.push_back(*prunedGoalIter);
469 
470  // Add as a sample
471  this->addToSamples(*prunedGoalIter);
472 
473  // Mark what we've added:
474  addedGoal = true;
475 
476  // Remove this goal from the vector of pruned vertices.
477  // Swap it to the element before our *new* end
478  if (prunedGoalIter != (prunedGoalEnd - 1))
479  {
480  std::swap(*prunedGoalIter, *(prunedGoalEnd - 1));
481  }
482 
483  // Move the end forward by one, marking it to be deleted
484  --prunedGoalEnd;
485 
486  // Leave the iterator where it is, as we need to recheck this element that we pulled from the
487  // back
488  }
489  }
490 
491  // Erase any elements moved to the "new end" of the vector
492  if (prunedGoalEnd != prunedGoalVertices_.end())
493  {
494  prunedGoalVertices_.erase(prunedGoalEnd, prunedGoalVertices_.end());
495  }
496  // No else, nothing to delete
497  }
498 
499  // Similarly, if we added a goal and have previously pruned starts, we will have to do the same on those
500  if (addedGoal && !prunedStartVertices_.empty())
501  {
502  // Variable
503  // An iterator to the vector of pruned starts
504  auto prunedStartIter = prunedStartVertices_.begin();
505  // The end point of the vector to consider. We will delete by swapping elements to the end, moving this
506  // iterator towards the start, and then erasing once at the end.
507  auto prunedStartEnd = prunedStartVertices_.end();
508 
509  // Consider each one
510  while (prunedStartIter != prunedStartEnd)
511  {
512  // Mark as unpruned
513  (*prunedStartIter)->markUnpruned();
514 
515  // Check if it should be readded (i.e., would it be pruned *now*?)
516  if (this->canVertexBeDisconnected(*prunedStartIter))
517  {
518  // It would be pruned, so remark as pruned
519  (*prunedStartIter)->markPruned();
520 
521  // and move onto the next:
522  ++prunedStartIter;
523  }
524  else
525  {
526  // It would not be pruned, readd it!
527  // Add it back to the vector
528  startVertices_.push_back(*prunedStartIter);
529 
530  // Add this vertex to the queue.
531  // queuePtr_->enqueueVertex(*prunedStartIter);
532 
533  // Add the vertex to the set of vertices.
534  this->registerAsVertex(*prunedStartIter);
535 
536  // Mark what we've added:
537  addedStart = true;
538 
539  // Remove this start from the vector of pruned vertices.
540  // Swap it to the element before our *new* end
541  if (prunedStartIter != (prunedStartEnd - 1))
542  {
543  std::swap(*prunedStartIter, *(prunedStartEnd - 1));
544  }
545 
546  // Move the end forward by one, marking it to be deleted
547  --prunedStartEnd;
548 
549  // Leave the iterator where it is, as we need to recheck this element that we pulled from the
550  // back
551  }
552  }
553 
554  // Erase any elements moved to the "new end" of the vector
555  if (prunedStartEnd != prunedStartVertices_.end())
556  {
557  prunedStartVertices_.erase(prunedStartEnd, prunedStartVertices_.end());
558  }
559  // No else, nothing to delete
560  }
561 
562  // If we've added anything, we have some updating to do.
563  if (addedGoal || addedStart)
564  {
565  // Update the minimum cost
566  for (const auto &startVertex : startVertices_)
567  {
568  // Take the better of the min cost so far and the cost-to-go from this start
569  minCost_ = costHelpPtr_->betterCost(minCost_, costHelpPtr_->costToGoHeuristic(startVertex));
570  }
571 
572  // If we have at least one start and goal, allocate a sampler
573  if (!startVertices_.empty() && !goalVertices_.empty())
574  {
575  // There is a start and goal, allocate
576  sampler_ = costHelpPtr_->getOptObj()->allocInformedStateSampler(
577  problemDefinition_, std::numeric_limits<unsigned int>::max());
578  }
579  // No else, this will get allocated when we get the updated start/goal.
580 
581  // Iterate through the existing vertices and find the current best approximate solution (if enabled)
582  if (!hasExactSolution_ && findApprox_)
583  {
584  this->updateVertexClosestToGoal();
585  }
586  }
587  // No else, why were we called?
588 
589  // Make sure that if we have a goal, we also have a start, since there's no way to wait for more *starts*
590  if (!goalVertices_.empty() && startVertices_.empty())
591  {
592  OMPL_WARN("%s (ImplicitGraph): The problem has a goal but not a start. BIT* cannot find a solution "
593  "since PlannerInputStates provides no method to wait for a valid _start_ state to appear.",
594  nameFunc_().c_str());
595  }
596  // No else
597  }
598 
599  void BITstar::ImplicitGraph::addNewSamples(const unsigned int &numSamples)
600  {
601  ASSERT_SETUP
602 
603  // Set the cost sampled to identity
604  sampledCost_ = costHelpPtr_->identityCost();
605 
606  // Store the number of samples being used in this batch
607  numNewSamplesInCurrentBatch_ = numSamples;
608 
609  // Update the nearest-neighbour terms for the number of samples we *will* have.
610  this->updateNearestTerms();
611 
612  // Add the recycled samples to the nearest neighbours struct.
613  samples_->add(recycledSamples_);
614 
615  // These recycled samples are our only new samples.
616  newSamples_ = recycledSamples_;
617 
618  // And there are no longer and recycled samples.
619  recycledSamples_.clear();
620 
621  // Increment the approximation id.
622  ++(*approximationId_);
623 
624  // We don't add actual *new* samples until the next time "nearestSamples" is called. This is to support JIT
625  // sampling.
626  }
627 
628  std::pair<unsigned int, unsigned int> BITstar::ImplicitGraph::prune(double prunedMeasure)
629  {
630  ASSERT_SETUP
631 
632 #ifdef BITSTAR_DEBUG
633  if (!hasExactSolution_)
634  {
635  throw ompl::Exception("A graph cannot be pruned until a solution is found");
636  }
637 #endif // BITSTAR_DEBUG
638 
639  // Store the measure of the problem I'm approximating
640  approximationMeasure_ = prunedMeasure;
641 
642  // Prune the starts/goals
643  std::pair<unsigned int, unsigned int> numPruned = this->pruneStartAndGoalVertices();
644 
645  // Prune the samples.
646  numPruned = numPruned + this->pruneSamples();
647 
648  return numPruned;
649  }
650 
652  {
653  ASSERT_SETUP
654 
655  // NO COUNTER. generated samples are counted at the sampler.
656 
657  // Add to the vector of new samples
658  newSamples_.push_back(sample);
659 
660  // Add to the NN structure:
661  samples_->add(sample);
662  }
663 
665  {
666  ASSERT_SETUP
667 
668  // NO COUNTER. generated samples are counted at the sampler.
669 
670  // Add to the vector of new samples
671  newSamples_.insert(newSamples_.end(), samples.begin(), samples.end());
672 
673  // Add to the NN structure:
674  samples_->add(samples);
675  }
676 
678  {
679  ASSERT_SETUP
680 
681  // Remove from the set of samples
682  samples_->remove(sample);
683  }
684 
686  {
687  ASSERT_SETUP
688 
689  // Variable:
690 #ifdef BITSTAR_DEBUG
691  // The use count of the passed shared pointer. Used in debug mode to assert that we took ownership of our
692  // own copy.
693  unsigned int initCount = sample.use_count();
694 #endif // BITSTAR_DEBUG
695  // A copy of the sample pointer to be removed so we can't delete it out from under ourselves (occurs when
696  // this function is given an element of the maintained set as the argument)
697  VertexPtr sampleCopy(sample);
698 
699 #ifdef BITSTAR_DEBUG
700  // Assert that the vertexToDelete took it's own copy
701  if (sampleCopy.use_count() <= initCount)
702  {
703  throw ompl::Exception("A code change has prevented ImplicitGraph::removeSample() "
704  "from taking it's own copy of the given shared pointer. See "
705  "https://github.com/ompl/ompl/issues/485");
706  }
707  if (sampleCopy->edgeQueueOutLookupSize() != 0u)
708  {
709  throw ompl::Exception("Encountered a sample with outgoing edges in the queue.");
710  }
711 #endif // BITSTAR_DEBUG
712 
713  // Remove all incoming edges from the search queue.
714  queuePtr_->removeInEdgesConnectedToVertexFromQueue(sampleCopy);
715 
716  // Remove from the set of samples
717  samples_->remove(sampleCopy);
718 
719  // Increment our counter
720  ++numFreeStatesPruned_;
721 
722  // Mark the sample as pruned
723  sampleCopy->markPruned();
724  }
725 
727  {
728  ASSERT_SETUP
729 
730  recycledSamples_.push_back(sample);
731  }
732 
734  {
735  ASSERT_SETUP
736 #ifdef BITSTAR_DEBUG
737  // Make sure it's connected first, so that the queue gets updated properly.
738  // This is a day of debugging I'll never get back
739  if (!vertex->isInTree())
740  {
741  throw ompl::Exception("Vertices must be connected to the graph before adding");
742  }
743 #endif // BITSTAR_DEBUG
744 
745  // Increment the number of vertices added:
746  ++numVertices_;
747 
748  // Update the nearest vertex to the goal (if tracking)
749  if (!hasExactSolution_ && findApprox_)
750  {
751  this->testClosestToGoal(vertex);
752  }
753  }
754 
755  unsigned int BITstar::ImplicitGraph::removeFromVertices(const VertexPtr &vertex, bool moveToFree)
756  {
757  ASSERT_SETUP
758 
759  // Variable:
760 #ifdef BITSTAR_DEBUG
761  // The use count of the passed shared pointer. Used in debug mode to assert that we took ownership of our
762  // own copy.
763  unsigned int initCount = vertex.use_count();
764 #endif // BITSTAR_DEBUG
765  // A copy of the vertex pointer to be removed so we can't delete it out from under ourselves (occurs when
766  // this function is given an element of the maintained set as the argument)
767  VertexPtr vertexCopy(vertex);
768 
769 #ifdef BITSTAR_DEBUG
770  // Assert that the vertexToDelete took it's own copy
771  if (vertexCopy.use_count() <= initCount)
772  {
773  throw ompl::Exception("A code change has prevented ImplicitGraph::removeVertex() "
774  "from taking it's own copy of the given shared pointer. See "
775  "https://github.com/ompl/ompl/issues/485");
776  }
777 #endif // BITSTAR_DEBUG
778 
779  // Increment our counter
780  ++numVerticesDisconnected_;
781 
782  // Remove from the nearest-neighbour structure
783  samples_->remove(vertexCopy);
784 
785  // Add back as sample, if that would be beneficial
786  if (moveToFree && !this->canSampleBePruned(vertexCopy))
787  {
788  // Yes, the vertex is still useful as a sample. Track as recycled so they are reused as samples in the
789  // next batch.
790  recycledSamples_.push_back(vertexCopy);
791 
792  // Return that the vertex was recycled
793  return 0u;
794  }
795  else
796  {
797  // No, the vertex is not useful anymore. Mark as pruned. This functions as a lock to prevent accessing
798  // anything about the vertex.
799  vertexCopy->markPruned();
800 
801  // Return that the vertex was completely pruned
802  return 1u;
803  }
804  }
805 
806  std::pair<unsigned int, unsigned int> BITstar::ImplicitGraph::pruneVertex(const VertexPtr &vertex)
807  {
808  ASSERT_SETUP
809 
810  // Variable:
811 #ifdef BITSTAR_DEBUG
812  // The use count of the passed shared pointer. Used in debug mode to assert that we took ownership of our
813  // own copy.
814  unsigned int initCount = vertex.use_count();
815 #endif // BITSTAR_DEBUG
816  // A copy of the sample pointer to be removed so we can't delete it out from under ourselves (occurs when
817  // this function is given an element of the maintained set as the argument)
818  VertexPtr vertexCopy(vertex);
819 
820 #ifdef BITSTAR_DEBUG
821  // Assert that the vertexToDelete took it's own copy
822  if (vertexCopy.use_count() <= initCount)
823  {
824  throw ompl::Exception("A code change has prevented ImplicitGraph::removeSample() "
825  "from taking it's own copy of the given shared pointer. See "
826  "https://github.com/ompl/ompl/issues/485");
827  }
828 #endif // BITSTAR_DEBUG
829 
830  // Remove from the set of inconsistent vertices if the vertex is inconsistent.
831  if (!vertexCopy->isConsistent())
832  {
833  queuePtr_->removeFromInconsistentSet(vertexCopy);
834  }
835 
836  // Disconnect from parent if necessary, not cascading cost updates.
837  if (vertexCopy->hasParent())
838  {
839  this->removeEdgeBetweenVertexAndParent(vertexCopy, false);
840  }
841 
842  // Remove all children and let them know that their parent is pruned.
843  VertexPtrVector children;
844  vertexCopy->getChildren(&children);
845  for (const auto &child : children)
846  {
847  // Remove this edge.
848  vertexCopy->removeChild(child);
849  child->removeParent(false);
850 
851  // If the child is inconsistent, it needs to be removed from the set of inconsistent vertices.
852  if (!child->isConsistent())
853  {
854  queuePtr_->removeFromInconsistentSet(child);
855  }
856 
857  // If the child has outgoing edges in the queue, they need to be removed.
858  queuePtr_->removeOutEdgesConnectedToVertexFromQueue(child);
859  }
860 
861  // Remove any edges still in the queue.
862  queuePtr_->removeAllEdgesConnectedToVertexFromQueue(vertexCopy);
863 
864  // Remove this vertex from the set of samples.
865  samples_->remove(vertexCopy);
866 
867  // This state is now no longer considered a vertex, but could still be useful as sample.
868  if (this->canSampleBePruned(vertexCopy))
869  {
870  // It's not even useful as sample, mark it as pruned.
871  vertexCopy->markPruned();
872  return {0, 1}; // The vertex is actually removed.
873  }
874  else
875  {
876  // It is useful as sample and should be recycled.
877  recycleSample(vertexCopy);
878  return {1, 0}; // The vertex is only disconnected and recycled as sample.
879  }
880  }
881 
882  void BITstar::ImplicitGraph::removeEdgeBetweenVertexAndParent(const VertexPtr &child, bool cascadeCostUpdates)
883  {
884 #ifdef BITSTAR_DEBUG
885  if (!child->hasParent())
886  {
887  throw ompl::Exception("An orphaned vertex has been passed for disconnection. Something went wrong.");
888  }
889 #endif // BITSTAR_DEBUG
890 
891  // Check if my parent has already been pruned. This can occur if we're cascading child disconnections.
892  if (!child->getParent()->isPruned())
893  {
894  // If not, remove myself from my parent's vector of children, not updating down-stream costs
895  child->getParent()->removeChild(child);
896  }
897 
898  // Remove my parent link, cascading cost updates if requested:
899  child->removeParent(cascadeCostUpdates);
900  }
901 
903 
905  // Private functions:
906  void BITstar::ImplicitGraph::updateSamples(const VertexConstPtr &vertex)
907  {
908  // The required cost to contain the neighbourhood of this vertex.
909  ompl::base::Cost requiredCost = this->calculateNeighbourhoodCost(vertex);
910 
911  // Check if we need to generate new samples inorder to completely cover the neighbourhood of the vertex
912  if (costHelpPtr_->isCostBetterThan(sampledCost_, requiredCost))
913  {
914  // The total number of samples we wish to have.
915  unsigned int numRequiredSamples = 0u;
916 
917  // Get the measure of what we're sampling.
918  if (useJustInTimeSampling_)
919  {
920  // Calculate the sample density given the number of samples per batch and the measure of this batch
921  // by assuming that this batch will fill the same measure as the previous.
922  double sampleDensity = static_cast<double>(numNewSamplesInCurrentBatch_) / approximationMeasure_;
923 
924  // Convert that into the number of samples needed for this slice.
925  double numSamplesForSlice =
926  sampleDensity * sampler_->getInformedMeasure(sampledCost_, requiredCost);
927 
928  // The integer of the double are definitely sampled
929  numRequiredSamples = numSamples_ + static_cast<unsigned int>(numSamplesForSlice);
930 
931  // And the fractional part represents the probability of one more sample. I like being pedantic.
932  if (rng_.uniform01() <= (numSamplesForSlice - static_cast<double>(numRequiredSamples)))
933  {
934  // One more please.
935  ++numRequiredSamples;
936  }
937  // No else.
938  }
939  else
940  {
941  // We're generating all our samples in one batch. Do it to it.
942  numRequiredSamples = numSamples_ + numNewSamplesInCurrentBatch_;
943  }
944 
945  // Actually generate the new samples
946  VertexPtrVector newStates{};
947  newStates.reserve(numRequiredSamples);
948  for (std::size_t tries = 0u;
949  tries < averageNumOfAllowedFailedAttemptsWhenSampling_ * numRequiredSamples &&
950  numSamples_ < numRequiredSamples;
951  ++tries)
952  {
953  // Variable
954  // The new state:
955  auto newState =
956  std::make_shared<Vertex>(spaceInformation_, costHelpPtr_, queuePtr_, approximationId_);
957 
958  // Sample in the interval [costSampled_, costReqd):
959  if (sampler_->sampleUniform(newState->state(), sampledCost_, requiredCost))
960  {
961  // If the state is collision free, add it to the set of free states
962  ++numStateCollisionChecks_;
963  if (spaceInformation_->isValid(newState->state()))
964  {
965  newStates.push_back(newState);
966 
967  // Update the number of uniformly distributed states
968  ++numUniformStates_;
969 
970  // Update the number of sample
971  ++numSamples_;
972  }
973  // No else
974  }
975  }
976 
977  // Add the new state as a sample.
978  this->addToSamples(newStates);
979 
980  // Record the sampled cost space
981  sampledCost_ = requiredCost;
982  }
983  // No else, the samples are up to date
984  }
985 
986  void BITstar::ImplicitGraph::updateVertexClosestToGoal()
987  {
988  if (static_cast<bool>(samples_))
989  {
990  // Get all samples as a vector.
991  VertexPtrVector samples;
992  samples_->list(samples);
993 
994  // Iterate through them testing which of the ones in the tree is the closest to the goal.
995  for (const auto &sample : samples)
996  {
997  if (sample->isInTree())
998  {
999  this->testClosestToGoal(sample);
1000  }
1001  }
1002  }
1003  // No else, there are no vertices.
1004  }
1005 
1006  std::pair<unsigned int, unsigned int> BITstar::ImplicitGraph::pruneStartAndGoalVertices()
1007  {
1008  // Variable
1009  // The number of starts/goals disconnected from the tree/pruned
1010  std::pair<unsigned int, unsigned int> numPruned(0u, 0u);
1011 
1012  // Are there superfluous starts to prune?
1013  if (startVertices_.size() > 1u)
1014  {
1015  // Yes, Iterate through the vector
1016 
1017  // Variable
1018  // The iterator to the start:
1019  auto startIter = startVertices_.begin();
1020  // The end point of the vector to consider. We will delete by swapping elements to the end, moving this
1021  // iterator towards the start, and then erasing once at the end.
1022  auto startEnd = startVertices_.end();
1023 
1024  // Run until at the end:
1025  while (startIter != startEnd)
1026  {
1027  // Check if this start has met the criteria to be pruned
1028  if (this->canVertexBeDisconnected(*startIter))
1029  {
1030  // It has, remove the start vertex DO NOT consider it as a sample. It is marked as a root node,
1031  // so having it as a sample would cause all kinds of problems, also it shouldn't be possible for
1032  // it to ever be useful as a sample anyway, unless there is a very weird cost function in play.
1033  numPruned.second = numPruned.second + this->removeFromVertices(*startIter, false);
1034 
1035  // Count as a disconnected vertex
1036  ++numPruned.first;
1037 
1038  // Disconnect from parent if necessary, cascading cost updates.
1039  if ((*startIter)->hasParent())
1040  {
1041  this->removeEdgeBetweenVertexAndParent(*startIter, true);
1042  queuePtr_->removeOutEdgesConnectedToVertexFromQueue(*startIter);
1043  }
1044 
1045  // // Remove it from the vertex queue.
1046  // queuePtr_->unqueueVertex(*startIter);
1047 
1048  // Store the start vertex in the pruned vector, in case it later needs to be readded:
1049  prunedStartVertices_.push_back(*startIter);
1050 
1051  // Remove this start from the vector.
1052  // Swap it to the element before our *new* end
1053  if (startIter != (startEnd - 1))
1054  {
1055  using std::swap; // Enable Koenig Lookup.
1056  swap(*startIter, *(startEnd - 1));
1057  }
1058 
1059  // Move the end forward by one, marking it to be deleted
1060  --startEnd;
1061 
1062  // Leave the iterator where it is, as we need to recheck this element that we pulled from the
1063  // back
1064  }
1065  else
1066  {
1067  // Still valid, move to the next one:
1068  ++startIter;
1069  }
1070  }
1071 
1072  // Erase any elements moved to the "new end" of the vector
1073  if (startEnd != startVertices_.end())
1074  {
1075  startVertices_.erase(startEnd, startVertices_.end());
1076  }
1077  // No else, nothing to delete
1078  }
1079  // No else, can't prune 1 start.
1080 
1081  // Are there superfluous goals to prune?
1082  if (goalVertices_.size() > 1u)
1083  {
1084  // Yes, Iterate through the vector
1085 
1086  // Variable
1087  // The iterator to the start:
1088  auto goalIter = goalVertices_.begin();
1089  // The end point of the vector to consider. We will delete by swapping elements to the end, moving this
1090  // iterator towards the start, and then erasing once at the end.
1091  auto goalEnd = goalVertices_.end();
1092 
1093  // Run until at the end:
1094  while (goalIter != goalEnd)
1095  {
1096  // Check if this goal has met the criteria to be pruned
1097  if (this->canSampleBePruned(*goalIter))
1098  {
1099  // It has, remove the goal vertex completely
1100  // Check if this vertex is in the tree
1101  if ((*goalIter)->isInTree())
1102  {
1103  // Disconnect from parent if necessary, cascading cost updates.
1104  if ((*goalIter)->hasParent())
1105  {
1106  this->removeEdgeBetweenVertexAndParent(*goalIter, true);
1107  queuePtr_->removeOutEdgesConnectedToVertexFromQueue(*goalIter);
1108 
1109  // If the goal is inconsistent, it needs to be removed from the set of inconsistent
1110  // vertices.
1111  if (!(*goalIter)->isConsistent())
1112  {
1113  queuePtr_->removeFromInconsistentSet(*goalIter);
1114  }
1115  }
1116 
1117  // Remove it from the set of vertices, recycling if necessary.
1118  this->removeFromVertices(*goalIter, true);
1119 
1120  // and as a vertex, allowing it to move to the set of samples.
1121  numPruned.second = numPruned.second + this->removeFromVertices(*goalIter, true);
1122 
1123  // Count it as a disconnected vertex
1124  ++numPruned.first;
1125  }
1126  else
1127  {
1128  // It is not, so just it like a sample
1129  this->pruneSample(*goalIter);
1130 
1131  // Count a pruned sample
1132  ++numPruned.second;
1133  }
1134 
1135  // Store the start vertex in the pruned vector, in case it later needs to be readded:
1136  prunedGoalVertices_.push_back(*goalIter);
1137 
1138  // Remove this goal from the vector.
1139  // Swap it to the element before our *new* end
1140  if (goalIter != (goalEnd - 1))
1141  {
1142  std::swap(*goalIter, *(goalEnd - 1));
1143  }
1144 
1145  // Move the end forward by one, marking it to be deleted
1146  --goalEnd;
1147 
1148  // Leave the iterator where it is, as we need to recheck this element that we pulled from the
1149  // back
1150  }
1151  else
1152  {
1153  // The goal is still valid, get the next
1154  ++goalIter;
1155  }
1156  }
1157 
1158  // Erase any elements moved to the "new end" of the vector
1159  if (goalEnd != goalVertices_.end())
1160  {
1161  goalVertices_.erase(goalEnd, goalVertices_.end());
1162  }
1163  // No else, nothing to delete
1164  }
1165  // No else, can't prune 1 goal.
1166 
1167  // We don't need to update our approximate solution (if we're providing one) as we will only prune once a
1168  // solution exists.
1169 
1170  // Return the amount of work done
1171  return numPruned;
1172  }
1173 
1174  std::pair<unsigned int, unsigned int> BITstar::ImplicitGraph::pruneSamples()
1175  {
1176  // The number of samples pruned in this pass:
1177  std::pair<unsigned int, unsigned int> numPruned{0u, 0u};
1178 
1179  // Get the vector of samples.
1180  VertexPtrVector samples;
1181  samples_->list(samples);
1182 
1183  // Are we dropping samples anytime we prune?
1184  if (dropSamplesOnPrune_)
1185  {
1186  std::size_t numFreeStatesPruned = 0u;
1187  for (const auto &sample : samples)
1188  {
1189  if (!sample->isInTree())
1190  {
1191  this->pruneSample(sample);
1192  ++numPruned.second;
1193  ++numFreeStatesPruned;
1194  }
1195  }
1196 
1197  // Store the number of uniform samples.
1198  numUniformStates_ = 0u;
1199 
1200  // Increase the global counter.
1201  numFreeStatesPruned_ += numFreeStatesPruned;
1202  }
1203  else
1204  {
1205  // Iterate through the vector and remove any samples that should not be in the graph.
1206  for (const auto &sample : samples)
1207  {
1208  if (sample->isInTree())
1209  {
1210  if (this->canVertexBeDisconnected(sample))
1211  {
1212  numPruned = numPruned + this->pruneVertex(sample);
1213  }
1214  }
1215  // Check if this state should be pruned.
1216  else if (this->canSampleBePruned(sample))
1217  {
1218  // It should, remove the sample from the NN structure.
1219  this->pruneSample(sample);
1220 
1221  // Keep track of how many are pruned.
1222  ++numPruned.second;
1223  }
1224  // No else, keep sample.
1225  }
1226  }
1227 
1228  return numPruned;
1229  }
1230 
1232  {
1233  ASSERT_SETUP
1234 
1235  // Threshold should always be g_t(x_g)
1236 
1237  // Prune the vertex if it could cannot part of a better solution in the current graph. Greater-than just in
1238  // case we're using an edge that is exactly optimally connected.
1239  // g_t(v) + h^(v) > g_t(x_g)?
1240  return costHelpPtr_->isCostWorseThan(costHelpPtr_->currentHeuristicVertex(vertex), solutionCost_);
1241  }
1242 
1244  {
1245  ASSERT_SETUP
1246 
1247 #ifdef BITSTAR_DEBUG
1248  if (sample->isPruned())
1249  {
1250  throw ompl::Exception("Asking whether a pruned sample can be pruned.");
1251  }
1252 #endif // BITSTAR_DEBUG
1253 
1254  // Threshold should always be g_t(x_g)
1255  // Prune the unconnected sample if it could never be better of a better solution.
1256  // g^(v) + h^(v) >= g_t(x_g)?
1257  return costHelpPtr_->isCostWorseThanOrEquivalentTo(costHelpPtr_->lowerBoundHeuristicVertex(sample),
1258  solutionCost_);
1259  }
1260 
1261  void BITstar::ImplicitGraph::testClosestToGoal(const VertexConstPtr &vertex)
1262  {
1263  // Find the shortest distance between the given vertex and a goal
1264  double distance = std::numeric_limits<double>::max();
1265  problemDefinition_->getGoal()->isSatisfied(vertex->state(), &distance);
1266 
1267  // Compare to the current best approximate solution
1268  if (distance < closestDistanceToGoal_)
1269  {
1270  // Better, update the approximate solution.
1271  closestVertexToGoal_ = vertex;
1272  closestDistanceToGoal_ = distance;
1273  }
1274  // No else, don't update if worse.
1275  }
1276 
1277  ompl::base::Cost BITstar::ImplicitGraph::calculateNeighbourhoodCost(const VertexConstPtr &vertex) const
1278  {
1279 #ifdef BITSTAR_DEBUG
1280  if (vertex->isPruned())
1281  {
1282  throw ompl::Exception("Calculating the neighbourhood cost of a pruned vertex.");
1283  }
1284 #endif // BITSTAR_DEBUG
1285  // Are we using JIT sampling?
1286  if (useJustInTimeSampling_)
1287  {
1288  // We are, return the maximum heuristic cost that represents a sample in the neighbourhood of the given
1289  // vertex.
1290  // There is no point generating samples worse the best solution (maxCost_) even if those samples are in
1291  // this vertex's neighbourhood.
1292  return costHelpPtr_->betterCost(
1293  solutionCost_, costHelpPtr_->combineCosts(costHelpPtr_->lowerBoundHeuristicVertex(vertex),
1294  ompl::base::Cost(2.0 * r_)));
1295  }
1296 
1297  // We are not, return the maximum cost we'd ever want to sample
1298  return solutionCost_;
1299  }
1300 
1301  void BITstar::ImplicitGraph::updateNearestTerms()
1302  {
1303  // The number of uniformly distributed states.
1304  unsigned int numUniformStates = 0u;
1305 
1306  // Calculate N, are we dropping samples?
1307  if (dropSamplesOnPrune_)
1308  {
1309  // We are, so we've been tracking the number of uniform states, just use that
1310  numUniformStates = numUniformStates_;
1311  }
1312  else if (isPruningEnabled_)
1313  {
1314  // We are not dropping samples but pruning is enabled, then all samples are uniform.
1315  numUniformStates = samples_->size();
1316  }
1317  else
1318  {
1319  // We don't prune, so we have to check how many samples are in the informed set.
1320  numUniformStates = computeNumberOfSamplesInInformedSet();
1321  }
1322 
1323  // If this is the first batch, we will be calculating the connection limits from only the starts and goals
1324  // for an RGG with m samples. That will be a complex graph. In this case, let us calculate the connection
1325  // limits considering the samples about to be generated. Doing so is equivalent to setting an upper-bound on
1326  // the radius, which RRT* does with it's min(maxEdgeLength, RGG-radius).
1327  if (numUniformStates == (startVertices_.size() + goalVertices_.size()))
1328  {
1329  numUniformStates = numUniformStates + numNewSamplesInCurrentBatch_;
1330  }
1331 
1332  // Now update the appropriate term
1333  if (useKNearest_)
1334  {
1335  k_ = this->calculateK(numUniformStates);
1336  }
1337  else
1338  {
1339  r_ = this->calculateR(numUniformStates);
1340  }
1341  }
1342 
1343  std::size_t BITstar::ImplicitGraph::computeNumberOfSamplesInInformedSet() const
1344  {
1345  // Get the samples as a vector.
1346  std::vector<VertexPtr> samples;
1347  samples_->list(samples);
1348 
1349  // Return the number of samples that can not be pruned.
1350  return std::count_if(samples.begin(), samples.end(),
1351  [this](const VertexPtr &sample) { return !canSampleBePruned(sample); });
1352  }
1353 
1354  double BITstar::ImplicitGraph::calculateR(unsigned int numUniformSamples) const
1355  {
1356  // Cast to double for readability. (?)
1357  auto stateDimension = static_cast<double>(spaceInformation_->getStateDimension());
1358  auto graphCardinality = static_cast<double>(numUniformSamples);
1359 
1360  // Calculate the term and return.
1361  return rewireFactor_ * this->calculateMinimumRggR() *
1362  std::pow(std::log(graphCardinality) / graphCardinality, 1.0 / stateDimension);
1363  }
1364 
1365  unsigned int BITstar::ImplicitGraph::calculateK(unsigned int numUniformSamples) const
1366  {
1367  // Calculate the term and return
1368  return std::ceil(rewireFactor_ * k_rgg_ * std::log(static_cast<double>(numUniformSamples)));
1369  }
1370 
1371  double BITstar::ImplicitGraph::calculateMinimumRggR() const
1372  {
1373  // Variables
1374  // The dimension cast as a double for readibility;
1375  auto stateDimension = static_cast<double>(spaceInformation_->getStateDimension());
1376 
1377  // Calculate the term and return
1378  // RRT*
1379  return std::pow(2.0 * (1.0 + 1.0 / stateDimension) *
1380  (approximationMeasure_ / unitNBallMeasure(spaceInformation_->getStateDimension())),
1381  1.0 / stateDimension);
1382 
1383  // Relevant work on calculating the minimum radius:
1384  /*
1385  // PRM*,RRG radius (biggest for unit-volume problem)
1386  // See Theorem 34 in Karaman & Frazzoli IJRR 2011
1387  return 2.0 * std::pow((1.0 + 1.0 / dimDbl) *
1388  (approximationMeasure_ /
1389  unitNBallMeasure(si_->getStateDimension())),
1390  1.0 / dimDbl);
1391 
1392  // FMT* radius (R2: smallest, R3: equiv to RRT*, Higher d: middle).
1393  // See Theorem 4.1 in Janson et al. IJRR 2015
1394  return 2.0 * std::pow((1.0 / dimDbl) *
1395  (approximationMeasure_ /
1396  unitNBallMeasure(si_->getStateDimension())),
1397  1.0 / dimDbl);
1398 
1399  // RRT* radius (smallest for unit-volume problems above R3).
1400  // See Theorem 38 in Karaman & Frazzoli IJRR 2011
1401  return std::pow(2.0 * (1.0 + 1.0 / dimDbl) *
1402  (approximationMeasure_ /
1403  unitNBallMeasure(si_->getStateDimension())),
1404  1.0 / dimDbl);
1405  */
1406  }
1407 
1408  double BITstar::ImplicitGraph::calculateMinimumRggK() const
1409  {
1410  // The dimension cast as a double for readibility.
1411  auto stateDimension = static_cast<double>(spaceInformation_->getStateDimension());
1412 
1413  // Calculate the term and return.
1414  return boost::math::constants::e<double>() +
1415  (boost::math::constants::e<double>() / stateDimension); // RRG k-nearest
1416  }
1417 
1418  void BITstar::ImplicitGraph::assertSetup() const
1419  {
1420  if (!isSetup_)
1421  {
1422  throw ompl::Exception("BITstar::ImplicitGraph was used before it was setup.");
1423  }
1424  }
1426 
1428  // Boring sets/gets (Public):
1430  {
1431  return (!startVertices_.empty());
1432  }
1433 
1435  {
1436  return (!goalVertices_.empty());
1437  }
1438 
1439  BITstar::VertexPtrVector::const_iterator BITstar::ImplicitGraph::startVerticesBeginConst() const
1440  {
1441  return startVertices_.cbegin();
1442  }
1443 
1444  BITstar::VertexPtrVector::const_iterator BITstar::ImplicitGraph::startVerticesEndConst() const
1445  {
1446  return startVertices_.cend();
1447  }
1448 
1449  BITstar::VertexPtrVector::const_iterator BITstar::ImplicitGraph::goalVerticesBeginConst() const
1450  {
1451  return goalVertices_.cbegin();
1452  }
1453 
1454  BITstar::VertexPtrVector::const_iterator BITstar::ImplicitGraph::goalVerticesEndConst() const
1455  {
1456  return goalVertices_.cend();
1457  }
1458 
1460  {
1461  return minCost_;
1462  }
1463 
1465  {
1466  return sampler_->hasInformedMeasure();
1467  }
1468 
1470  {
1471  return sampler_->getInformedMeasure(cost);
1472  }
1473 
1475  {
1476 #ifdef BITSTAR_DEBUG
1477  if (!findApprox_)
1478  {
1479  throw ompl::Exception("Approximate solutions are not being tracked.");
1480  }
1481 #endif // BITSTAR_DEBUG
1482  return closestVertexToGoal_;
1483  }
1484 
1486  {
1487 #ifdef BITSTAR_DEBUG
1488  if (!findApprox_)
1489  {
1490  throw ompl::Exception("Approximate solutions are not being tracked.");
1491  }
1492 #endif // BITSTAR_DEBUG
1493  return closestDistanceToGoal_;
1494  }
1495 
1497  {
1498 #ifdef BITSTAR_DEBUG
1499  if (!useKNearest_)
1500  {
1501  throw ompl::Exception("Using an r-disc graph.");
1502  }
1503 #endif // BITSTAR_DEBUG
1504  return k_;
1505  }
1506 
1508  {
1509 #ifdef BITSTAR_DEBUG
1510  if (useKNearest_)
1511  {
1512  throw ompl::Exception("Using a k-nearest graph.");
1513  }
1514 #endif // BITSTAR_DEBUG
1515  return r_;
1516  }
1517 
1519  {
1520  VertexPtrVector samples;
1521  samples_->list(samples);
1522  return samples;
1523  }
1524 
1526  {
1527  // Store
1528  rewireFactor_ = rewireFactor;
1529 
1530  // Check if there's things to update
1531  if (isSetup_)
1532  {
1533  // Reinitialize the terms:
1534  this->updateNearestTerms();
1535  }
1536  }
1537 
1539  {
1540  return rewireFactor_;
1541  }
1542 
1544  {
1545  // Assure that we're not trying to enable k-nearest with JIT sampling already on
1546  if (useKNearest && useJustInTimeSampling_)
1547  {
1548  OMPL_WARN("%s (ImplicitGraph): The k-nearest variant of BIT* cannot be used with JIT sampling, "
1549  "continuing to use the r-disc variant.",
1550  nameFunc_().c_str());
1551  }
1552  else
1553  {
1554  // Store
1555  useKNearest_ = useKNearest;
1556 
1557  // Check if there's things to update
1558  if (isSetup_)
1559  {
1560  // Calculate the current term:
1561  this->updateNearestTerms();
1562  }
1563  }
1564  }
1565 
1567  {
1568  return useKNearest_;
1569  }
1570 
1572  {
1573  // Assure that we're not trying to enable k-nearest with JIT sampling already on
1574  if (useKNearest_ && useJit)
1575  {
1576  OMPL_WARN("%s (ImplicitGraph): Just-in-time sampling cannot be used with the k-nearest variant of "
1577  "BIT*, continuing to use regular sampling.",
1578  nameFunc_().c_str());
1579  }
1580  else
1581  {
1582  // Store
1583  useJustInTimeSampling_ = useJit;
1584 
1585  // Announce limitation:
1586  if (useJit)
1587  {
1588  OMPL_INFORM("%s (ImplicitGraph): Just-in-time sampling is currently only implemented for problems "
1589  "seeking to minimize path-length.",
1590  nameFunc_().c_str());
1591  }
1592  // No else
1593  }
1594  }
1595 
1597  {
1598  return useJustInTimeSampling_;
1599  }
1600 
1602  {
1603  // Make sure we're not already setup
1604  if (isSetup_)
1605  {
1606  OMPL_WARN("%s (ImplicitGraph): Periodic sample removal cannot be changed once BIT* is setup. "
1607  "Continuing to use the previous setting.",
1608  nameFunc_().c_str());
1609  }
1610  else
1611  {
1612  // Store
1613  dropSamplesOnPrune_ = dropSamples;
1614  }
1615  }
1616 
1618  {
1619  isPruningEnabled_ = usePruning;
1620  }
1621 
1623  {
1624  return dropSamplesOnPrune_;
1625  }
1626 
1628  {
1629  // Is the flag changing?
1630  if (findApproximate != findApprox_)
1631  {
1632  // Store the flag
1633  findApprox_ = findApproximate;
1634 
1635  // Check if we are enabling or disabling approximate solution support
1636  if (!findApprox_)
1637  {
1638  // We're turning it off, clear the approximate solution variables:
1639  closestDistanceToGoal_ = std::numeric_limits<double>::infinity();
1640  closestVertexToGoal_.reset();
1641  }
1642  else
1643  {
1644  // We are turning it on, do we have an exact solution?
1645  if (!hasExactSolution_)
1646  {
1647  // We don't, find our current best approximate solution:
1648  this->updateVertexClosestToGoal();
1649  }
1650  // No else, exact is better than approximate.
1651  }
1652  }
1653  // No else, no change.
1654  }
1655 
1657  {
1658  return findApprox_;
1659  }
1660 
1662  {
1663  averageNumOfAllowedFailedAttemptsWhenSampling_ = number;
1664  }
1665 
1667  {
1668  return averageNumOfAllowedFailedAttemptsWhenSampling_;
1669  }
1670 
1671  template <template <typename T> class NN>
1673  {
1674  // Check if the problem is already setup, if so, the NN structs have data in them and you can't really
1675  // change them:
1676  if (isSetup_)
1677  {
1678  OMPL_WARN("%s (ImplicitGraph): The nearest neighbour datastructures cannot be changed once the problem "
1679  "is setup. Continuing to use the existing containers.",
1680  nameFunc_().c_str());
1681  }
1682  else
1683  {
1684  // The problem isn't setup yet, create NN structs of the specified type:
1685  samples_ = std::make_shared<NN<VertexPtr>>();
1686  }
1687  }
1688 
1690  {
1691  return samples_->size();
1692  }
1693 
1695  {
1696  VertexPtrVector samples;
1697  samples_->list(samples);
1698  return std::count_if(samples.begin(), samples.end(),
1699  [](const VertexPtr &sample) { return sample->isInTree(); });
1700  }
1701 
1703  {
1704  return numSamples_;
1705  }
1706 
1708  {
1709  return numVertices_;
1710  }
1711 
1713  {
1714  return numFreeStatesPruned_;
1715  }
1716 
1718  {
1719  return numVerticesDisconnected_;
1720  }
1721 
1723  {
1724  return numNearestNeighbours_;
1725  }
1726 
1728  {
1729  return numStateCollisionChecks_;
1730  }
1732  } // namespace geometric
1733 } // namespace ompl
void nearestSamples(const VertexPtr &vertex, VertexPtrVector *neighbourSamples)
Get the nearest unconnected samples using the appropriate "near" definition (i.e.,...
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:89
PlannerTerminationCondition plannerAlwaysTerminatingCondition()
Simple termination condition that always returns true. The termination condition will always be met.
void setAverageNumOfAllowedFailedAttemptsWhenSampling(std::size_t number)
Set the average number of allowed failed attempts when sampling.
bool haveMoreStartStates() const
Check if there are more potential start states.
Definition: Planner.cpp:339
Base class for a planner.
Definition: Planner.h:279
void addToSamples(const VertexPtr &sample)
Add an unconnected sample.
void setDropSamplesOnPrune(bool dropSamples)
Set whether unconnected samples are dropped on pruning.
unsigned int numStatesGenerated() const
The total number of states generated.
void setRewireFactor(double rewireFactor)
Set the rewiring scale factor, s, such that r_rrg = s \times r_rrg*.
bool hasAStart() const
Gets whether the graph contains a start or not.
void setTrackApproximateSolutions(bool findApproximate)
Set whether to track approximate solutions during the search.
A shared pointer wrapper for ompl::base::SpaceInformation.
Helper class to extract valid start & goal states. Usually used internally by planners.
Definition: Planner.h:141
VertexPtrVector::const_iterator goalVerticesBeginConst() const
Returns a const-iterator to the front of the goal-vertex vector.
VertexPtrVector::const_iterator goalVerticesEndConst() const
Returns a const-iterator to the end of the goal-vertex vector.
std::shared_ptr< const Vertex > VertexConstPtr
A shared pointer to a const vertex.
Definition: BITstar.h:215
unsigned int numVertices() const
The number of vertices in the search tree.
A queue of edges, sorted according to a sort key.
Definition: SearchQueue.h:128
Definition of an abstract state.
Definition: State.h:113
bool hasAGoal() const
Gets whether the graph contains a goal or not.
void setUseKNearest(bool useKNearest)
Enable a k-nearest search for instead of an r-disc search.
void getGraphAsPlannerData(ompl::base::PlannerData &data) const
Adds the graph to the given PlannerData struct.
std::function< std::string()> NameFunc
A utility functor for ImplicitGraph and SearchQueue.
Definition: BITstar.h:245
std::pair< VertexConstPtr, VertexConstPtr > VertexConstPtrPair
A pair of const vertices, i.e., an edge.
Definition: BITstar.h:233
void registerAsVertex(const VertexPtr &vertex)
Add a vertex to the tree, optionally moving it from the set of unconnected samples.
bool getUseKNearest() const
Get whether a k-nearest search is being used.
void log(const char *file, int line, LogLevel level, const char *m,...)
Root level logging function. This should not be invoked directly, but rather used via a logging macro...
Definition: Console.cpp:120
VertexPtrVector getCopyOfSamples() const
Get a copy of all samples.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
void setPruning(bool usePruning)
Set whether samples that are provably not beneficial should be kept around.
void setup(const ompl::base::SpaceInformationPtr &spaceInformation, const ompl::base::ProblemDefinitionPtr &problemDefinition, CostHelper *costHelper, SearchQueue *searchQueue, const ompl::base::Planner *plannerPtr, ompl::base::PlannerInputStates &inputStates)
Setup the ImplicitGraph, must be called before use. Does not take a copy of the PlannerInputStates,...
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
VertexPtrVector::const_iterator startVerticesEndConst() const
Returns a const-iterator to the end of the start-vertex vector.
unsigned int numVerticesDisconnected() const
The number of tree vertices disconnected.
A helper class to handle the various heuristic functions in one place.
Definition: CostHelper.h:133
bool canSampleBePruned(const VertexPtr &sample) const
Returns whether the sample can be pruned, i.e., whether it could ever provide a better solution....
std::pair< unsigned int, unsigned int > pruneVertex(const VertexPtr &vertex)
Remove a vertex and mark as pruned.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
ImplicitGraph(NameFunc nameFunc)
Construct an implicit graph.
double unitNBallMeasure(unsigned int N)
The Lebesgue measure (i.e., "volume") of an n-dimensional ball with a unit radius.
double getInformedMeasure(const ompl::base::Cost &cost) const
Query the underlying state sampler for the informed measure of the problem.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:228
void removeFromSamples(const VertexPtr &sample)
Remove a sample from the sample set.
double distance(const VertexConstPtr &a, const VertexConstPtr &b) const
Computes the distance between two states.
unsigned int numVerticesConnected() const
The total number of vertices added to the graph.
std::vector< VertexPtr > VertexPtrVector
A vector of shared pointers to vertices.
Definition: BITstar.h:221
const State * nextGoal(const PlannerTerminationCondition &ptc)
Return the next valid goal state or nullptr if no more valid goal states are available....
Definition: Planner.cpp:265
Abstract representation of a container that can perform nearest neighbors queries.
VertexConstPtr closestVertexToGoal() const
IF BEING TRACKED, returns the closest vertex in the tree to the goal.
void removeEdgeBetweenVertexAndParent(const VertexPtr &child, bool cascadeCostUpdates)
Disconnect a vertex from its parent by removing the edges stored in itself, and its parents....
A shared pointer wrapper for ompl::base::ProblemDefinition.
void updateStartAndGoalStates(ompl::base::PlannerInputStates &inputStates, const base::PlannerTerminationCondition &terminationCondition)
Adds any new goals or starts that have appeared in the problem definition to the vector of vertices a...
unsigned int numStateCollisionChecks() const
The number of state collision checks.
std::shared_ptr< Vertex > VertexPtr
A shared pointer to a vertex.
Definition: BITstar.h:212
double getConnectivityR() const
Get the radius of this r-disc RGG.
std::size_t getAverageNumOfAllowedFailedAttemptsWhenSampling() const
Get the average number of allowed failed attempts when sampling.
unsigned int numFreeStatesPruned() const
The number of states pruned.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
unsigned int numNearestLookups() const
The number of nearest neighbour calls.
bool hasInformedMeasure() const
Query whether the underlying state sampler can provide an informed measure.
void reset()
Reset the queue to the state of construction.
void recycleSample(const VertexPtr &sample)
Insert a sample into the set for recycled samples.
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added....
void pruneSample(const VertexPtr &sample)
Remove an unconnected sample.
bool getJustInTimeSampling() const
Get whether we're using just-in-time sampling.
double smallestDistanceToGoal() const
IF BEING TRACKED, returns the how close vertices in the tree are to the goal.
void reset()
Reset the graph to the state of construction.
bool getTrackApproximateSolutions() const
Get whether approximate solutions are tracked during the search.
ompl::base::Cost minCost() const
Get the minimum cost solution possible for this problem.
bool getDropSamplesOnPrune() const
Get whether unconnected samples are dropped on pruning.
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
bool haveMoreGoalStates() const
Check if there are more potential goal states.
Definition: Planner.cpp:346
unsigned int removeFromVertices(const VertexPtr &sample, bool moveToFree)
Remove a vertex from the tree, can optionally be allowed to move it to the set of unconnected samples...
void setNearestNeighbors()
Set a different nearest neighbours datastructure.
unsigned int getConnectivityK() const
Get the k of this k-nearest RGG.
unsigned int numSamples() const
The number of samples.
bool canVertexBeDisconnected(const VertexPtr &vertex) const
Returns whether the vertex can be pruned, i.e., whether it could provide a better solution given....
std::pair< unsigned int, unsigned int > prune(double prunedMeasure)
Prune the samples to the subproblem of the given measure. Returns the number of vertices disconnected...
void registerSolutionCost(const ompl::base::Cost &solutionCost)
Mark that a solution has been found and that the graph should be limited to the given heuristic value...
VertexPtrVector::const_iterator startVerticesBeginConst() const
Returns a const-iterator to the front of the start-vertex vector.
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
double getRewireFactor() const
Get the rewiring scale factor.
The exception type for ompl.
Definition: Exception.h:78
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:122
Main namespace. Contains everything in this library.
virtual void setDistanceFunction(const DistanceFunction &distFun)
Set the distance function to use.
void addNewSamples(const unsigned int &numSamples)
Increase the resolution of the graph-based approximation of the continuous search domain by adding a ...