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 as a sample
531  this->addToSamples(*prunedStartIter);
532 
533  // Add this vertex to the queue.
534  // queuePtr_->enqueueVertex(*prunedStartIter);
535 
536  // Add the vertex to the set of vertices.
537  this->registerAsVertex(*prunedStartIter);
538 
539  // Mark what we've added:
540  addedStart = true;
541 
542  // Remove this start from the vector of pruned vertices.
543  // Swap it to the element before our *new* end
544  if (prunedStartIter != (prunedStartEnd - 1))
545  {
546  std::swap(*prunedStartIter, *(prunedStartEnd - 1));
547  }
548 
549  // Move the end forward by one, marking it to be deleted
550  --prunedStartEnd;
551 
552  // Leave the iterator where it is, as we need to recheck this element that we pulled from the
553  // back
554  }
555  }
556 
557  // Erase any elements moved to the "new end" of the vector
558  if (prunedStartEnd != prunedStartVertices_.end())
559  {
560  prunedStartVertices_.erase(prunedStartEnd, prunedStartVertices_.end());
561  }
562  // No else, nothing to delete
563  }
564 
565  // If we've added anything, we have some updating to do.
566  if (addedGoal || addedStart)
567  {
568  // Update the minimum cost
569  for (const auto &startVertex : startVertices_)
570  {
571  // Take the better of the min cost so far and the cost-to-go from this start
572  minCost_ = costHelpPtr_->betterCost(minCost_, costHelpPtr_->costToGoHeuristic(startVertex));
573  }
574 
575  // If we have at least one start and goal, allocate a sampler
576  if (!startVertices_.empty() && !goalVertices_.empty())
577  {
578  // There is a start and goal, allocate
579  sampler_ = costHelpPtr_->getOptObj()->allocInformedStateSampler(
580  problemDefinition_, std::numeric_limits<unsigned int>::max());
581  }
582  // No else, this will get allocated when we get the updated start/goal.
583 
584  // Iterate through the existing vertices and find the current best approximate solution (if enabled)
585  if (!hasExactSolution_ && findApprox_)
586  {
587  this->updateVertexClosestToGoal();
588  }
589  }
590  // No else, why were we called?
591 
592  // Make sure that if we have a goal, we also have a start, since there's no way to wait for more *starts*
593  if (!goalVertices_.empty() && startVertices_.empty())
594  {
595  OMPL_WARN("%s (ImplicitGraph): The problem has a goal but not a start. BIT* cannot find a solution "
596  "since PlannerInputStates provides no method to wait for a valid _start_ state to appear.",
597  nameFunc_().c_str());
598  }
599  // No else
600  }
601 
602  void BITstar::ImplicitGraph::addNewSamples(const unsigned int &numSamples)
603  {
604  ASSERT_SETUP
605 
606  // Set the cost sampled to identity
607  sampledCost_ = costHelpPtr_->identityCost();
608 
609  // Store the number of samples being used in this batch
610  numNewSamplesInCurrentBatch_ = numSamples;
611 
612  // Update the nearest-neighbour terms for the number of samples we *will* have.
613  this->updateNearestTerms();
614 
615  // Add the recycled samples to the nearest neighbours struct.
616  samples_->add(recycledSamples_);
617 
618  // These recycled samples are our only new samples.
619  newSamples_ = recycledSamples_;
620 
621  // And there are no longer and recycled samples.
622  recycledSamples_.clear();
623 
624  // Increment the approximation id.
625  ++(*approximationId_);
626 
627  // We don't add actual *new* samples until the next time "nearestSamples" is called. This is to support JIT
628  // sampling.
629  }
630 
631  std::pair<unsigned int, unsigned int> BITstar::ImplicitGraph::prune(double prunedMeasure)
632  {
633  ASSERT_SETUP
634 
635 #ifdef BITSTAR_DEBUG
636  if (!hasExactSolution_)
637  {
638  throw ompl::Exception("A graph cannot be pruned until a solution is found");
639  }
640 #endif // BITSTAR_DEBUG
641 
642  // Store the measure of the problem I'm approximating
643  approximationMeasure_ = prunedMeasure;
644 
645  // Prune the starts/goals
646  std::pair<unsigned int, unsigned int> numPruned = this->pruneStartAndGoalVertices();
647 
648  // Prune the samples.
649  numPruned = numPruned + this->pruneSamples();
650 
651  return numPruned;
652  }
653 
655  {
656  ASSERT_SETUP
657 
658  // NO COUNTER. generated samples are counted at the sampler.
659 
660  // Add to the vector of new samples
661  newSamples_.push_back(sample);
662 
663  // Add to the NN structure:
664  samples_->add(sample);
665  }
666 
668  {
669  ASSERT_SETUP
670 
671  // NO COUNTER. generated samples are counted at the sampler.
672 
673  // Add to the vector of new samples
674  newSamples_.insert(newSamples_.end(), samples.begin(), samples.end());
675 
676  // Add to the NN structure:
677  samples_->add(samples);
678  }
679 
681  {
682  ASSERT_SETUP
683 
684  // Remove from the set of samples
685  samples_->remove(sample);
686  }
687 
689  {
690  ASSERT_SETUP
691 
692  // Variable:
693 #ifdef BITSTAR_DEBUG
694  // The use count of the passed shared pointer. Used in debug mode to assert that we took ownership of our
695  // own copy.
696  unsigned int initCount = sample.use_count();
697 #endif // BITSTAR_DEBUG
698  // A copy of the sample pointer to be removed so we can't delete it out from under ourselves (occurs when
699  // this function is given an element of the maintained set as the argument)
700  VertexPtr sampleCopy(sample);
701 
702 #ifdef BITSTAR_DEBUG
703  // Assert that the vertexToDelete took it's own copy
704  if (sampleCopy.use_count() <= initCount)
705  {
706  throw ompl::Exception("A code change has prevented ImplicitGraph::removeSample() "
707  "from taking it's own copy of the given shared pointer. See "
708  "https://github.com/ompl/ompl/issues/485");
709  }
710  if (sampleCopy->edgeQueueOutLookupSize() != 0u)
711  {
712  throw ompl::Exception("Encountered a sample with outgoing edges in the queue.");
713  }
714 #endif // BITSTAR_DEBUG
715 
716  // Remove all incoming edges from the search queue.
717  queuePtr_->removeInEdgesConnectedToVertexFromQueue(sampleCopy);
718 
719  // Remove from the set of samples
720  samples_->remove(sampleCopy);
721 
722  // Increment our counter
723  ++numFreeStatesPruned_;
724 
725  // Mark the sample as pruned
726  sampleCopy->markPruned();
727  }
728 
730  {
731  ASSERT_SETUP
732 
733  recycledSamples_.push_back(sample);
734  }
735 
737  {
738  ASSERT_SETUP
739 #ifdef BITSTAR_DEBUG
740  // Make sure it's connected first, so that the queue gets updated properly.
741  // This is a day of debugging I'll never get back
742  if (!vertex->isInTree())
743  {
744  throw ompl::Exception("Vertices must be connected to the graph before adding");
745  }
746 #endif // BITSTAR_DEBUG
747 
748  // Increment the number of vertices added:
749  ++numVertices_;
750 
751  // Update the nearest vertex to the goal (if tracking)
752  if (!hasExactSolution_ && findApprox_)
753  {
754  this->testClosestToGoal(vertex);
755  }
756  }
757 
758  unsigned int BITstar::ImplicitGraph::removeFromVertices(const VertexPtr &vertex, bool moveToFree)
759  {
760  ASSERT_SETUP
761 
762  // Variable:
763 #ifdef BITSTAR_DEBUG
764  // The use count of the passed shared pointer. Used in debug mode to assert that we took ownership of our
765  // own copy.
766  unsigned int initCount = vertex.use_count();
767 #endif // BITSTAR_DEBUG
768  // A copy of the vertex pointer to be removed so we can't delete it out from under ourselves (occurs when
769  // this function is given an element of the maintained set as the argument)
770  VertexPtr vertexCopy(vertex);
771 
772 #ifdef BITSTAR_DEBUG
773  // Assert that the vertexToDelete took it's own copy
774  if (vertexCopy.use_count() <= initCount)
775  {
776  throw ompl::Exception("A code change has prevented ImplicitGraph::removeVertex() "
777  "from taking it's own copy of the given shared pointer. See "
778  "https://github.com/ompl/ompl/issues/485");
779  }
780 #endif // BITSTAR_DEBUG
781 
782  // Increment our counter
783  ++numVerticesDisconnected_;
784 
785  // Remove from the nearest-neighbour structure
786  samples_->remove(vertexCopy);
787 
788  // Add back as sample, if that would be beneficial
789  if (moveToFree && !this->canSampleBePruned(vertexCopy))
790  {
791  // Yes, the vertex is still useful as a sample. Track as recycled so they are reused as samples in the
792  // next batch.
793  recycledSamples_.push_back(vertexCopy);
794 
795  // Return that the vertex was recycled
796  return 0u;
797  }
798  else
799  {
800  // No, the vertex is not useful anymore. Mark as pruned. This functions as a lock to prevent accessing
801  // anything about the vertex.
802  vertexCopy->markPruned();
803 
804  // Return that the vertex was completely pruned
805  return 1u;
806  }
807  }
808 
809  std::pair<unsigned int, unsigned int> BITstar::ImplicitGraph::pruneVertex(const VertexPtr &vertex)
810  {
811  ASSERT_SETUP
812 
813  // Variable:
814 #ifdef BITSTAR_DEBUG
815  // The use count of the passed shared pointer. Used in debug mode to assert that we took ownership of our
816  // own copy.
817  unsigned int initCount = vertex.use_count();
818 #endif // BITSTAR_DEBUG
819  // A copy of the sample pointer to be removed so we can't delete it out from under ourselves (occurs when
820  // this function is given an element of the maintained set as the argument)
821  VertexPtr vertexCopy(vertex);
822 
823 #ifdef BITSTAR_DEBUG
824  // Assert that the vertexToDelete took it's own copy
825  if (vertexCopy.use_count() <= initCount)
826  {
827  throw ompl::Exception("A code change has prevented ImplicitGraph::removeSample() "
828  "from taking it's own copy of the given shared pointer. See "
829  "https://github.com/ompl/ompl/issues/485");
830  }
831 #endif // BITSTAR_DEBUG
832 
833  // Remove from the set of inconsistent vertices if the vertex is inconsistent.
834  if (!vertexCopy->isConsistent())
835  {
836  queuePtr_->removeFromInconsistentSet(vertexCopy);
837  }
838 
839  // Disconnect from parent if necessary, not cascading cost updates.
840  if (vertexCopy->hasParent())
841  {
842  this->removeEdgeBetweenVertexAndParent(vertexCopy, false);
843  }
844 
845  // Remove all children and let them know that their parent is pruned.
846  VertexPtrVector children;
847  vertexCopy->getChildren(&children);
848  for (const auto &child : children)
849  {
850  // Remove this edge.
851  vertexCopy->removeChild(child);
852  child->removeParent(false);
853 
854  // If the child is inconsistent, it needs to be removed from the set of inconsistent vertices.
855  if (!child->isConsistent())
856  {
857  queuePtr_->removeFromInconsistentSet(child);
858  }
859 
860  // If the child has outgoing edges in the queue, they need to be removed.
861  queuePtr_->removeOutEdgesConnectedToVertexFromQueue(child);
862  }
863 
864  // Remove any edges still in the queue.
865  queuePtr_->removeAllEdgesConnectedToVertexFromQueue(vertexCopy);
866 
867  // Remove this vertex from the set of samples.
868  samples_->remove(vertexCopy);
869 
870  // This state is now no longer considered a vertex, but could still be useful as sample.
871  if (this->canSampleBePruned(vertexCopy))
872  {
873  // It's not even useful as sample, mark it as pruned.
874  vertexCopy->markPruned();
875  return {0, 1}; // The vertex is actually removed.
876  }
877  else
878  {
879  // It is useful as sample and should be recycled.
880  recycleSample(vertexCopy);
881  return {1, 0}; // The vertex is only disconnected and recycled as sample.
882  }
883  }
884 
885  void BITstar::ImplicitGraph::removeEdgeBetweenVertexAndParent(const VertexPtr &child, bool cascadeCostUpdates)
886  {
887 #ifdef BITSTAR_DEBUG
888  if (!child->hasParent())
889  {
890  throw ompl::Exception("An orphaned vertex has been passed for disconnection. Something went wrong.");
891  }
892 #endif // BITSTAR_DEBUG
893 
894  // Check if my parent has already been pruned. This can occur if we're cascading child disconnections.
895  if (!child->getParent()->isPruned())
896  {
897  // If not, remove myself from my parent's vector of children, not updating down-stream costs
898  child->getParent()->removeChild(child);
899  }
900 
901  // Remove my parent link, cascading cost updates if requested:
902  child->removeParent(cascadeCostUpdates);
903  }
904 
906 
908  // Private functions:
909  void BITstar::ImplicitGraph::updateSamples(const VertexConstPtr &vertex)
910  {
911  // The required cost to contain the neighbourhood of this vertex.
912  ompl::base::Cost requiredCost = this->calculateNeighbourhoodCost(vertex);
913 
914  // Check if we need to generate new samples inorder to completely cover the neighbourhood of the vertex
915  if (costHelpPtr_->isCostBetterThan(sampledCost_, requiredCost))
916  {
917  // The total number of samples we wish to have.
918  unsigned int numRequiredSamples = 0u;
919 
920  // Get the measure of what we're sampling.
921  if (useJustInTimeSampling_)
922  {
923  // Calculate the sample density given the number of samples per batch and the measure of this batch
924  // by assuming that this batch will fill the same measure as the previous.
925  double sampleDensity = static_cast<double>(numNewSamplesInCurrentBatch_) / approximationMeasure_;
926 
927  // Convert that into the number of samples needed for this slice.
928  double numSamplesForSlice =
929  sampleDensity * sampler_->getInformedMeasure(sampledCost_, requiredCost);
930 
931  // The integer of the double are definitely sampled
932  numRequiredSamples = numSamples_ + static_cast<unsigned int>(numSamplesForSlice);
933 
934  // And the fractional part represents the probability of one more sample. I like being pedantic.
935  if (rng_.uniform01() <= (numSamplesForSlice - static_cast<double>(numRequiredSamples)))
936  {
937  // One more please.
938  ++numRequiredSamples;
939  }
940  // No else.
941  }
942  else
943  {
944  // We're generating all our samples in one batch. Do it to it.
945  numRequiredSamples = numSamples_ + numNewSamplesInCurrentBatch_;
946  }
947 
948  // Actually generate the new samples
949  VertexPtrVector newStates{};
950  newStates.reserve(numRequiredSamples);
951  for (std::size_t tries = 0u;
952  tries < averageNumOfAllowedFailedAttemptsWhenSampling_ * numRequiredSamples &&
953  numSamples_ < numRequiredSamples;
954  ++tries)
955  {
956  // Variable
957  // The new state:
958  auto newState =
959  std::make_shared<Vertex>(spaceInformation_, costHelpPtr_, queuePtr_, approximationId_);
960 
961  // Sample in the interval [costSampled_, costReqd):
962  if (sampler_->sampleUniform(newState->state(), sampledCost_, requiredCost))
963  {
964  // If the state is collision free, add it to the set of free states
965  ++numStateCollisionChecks_;
966  if (spaceInformation_->isValid(newState->state()))
967  {
968  newStates.push_back(newState);
969 
970  // Update the number of uniformly distributed states
971  ++numUniformStates_;
972 
973  // Update the number of sample
974  ++numSamples_;
975  }
976  // No else
977  }
978  }
979 
980  // Add the new state as a sample.
981  this->addToSamples(newStates);
982 
983  // Record the sampled cost space
984  sampledCost_ = requiredCost;
985  }
986  // No else, the samples are up to date
987  }
988 
989  void BITstar::ImplicitGraph::updateVertexClosestToGoal()
990  {
991  if (static_cast<bool>(samples_))
992  {
993  // Get all samples as a vector.
994  VertexPtrVector samples;
995  samples_->list(samples);
996 
997  // Iterate through them testing which of the ones in the tree is the closest to the goal.
998  for (const auto &sample : samples)
999  {
1000  if (sample->isInTree())
1001  {
1002  this->testClosestToGoal(sample);
1003  }
1004  }
1005  }
1006  // No else, there are no vertices.
1007  }
1008 
1009  std::pair<unsigned int, unsigned int> BITstar::ImplicitGraph::pruneStartAndGoalVertices()
1010  {
1011  // Variable
1012  // The number of starts/goals disconnected from the tree/pruned
1013  std::pair<unsigned int, unsigned int> numPruned(0u, 0u);
1014 
1015  // Are there superfluous starts to prune?
1016  if (startVertices_.size() > 1u)
1017  {
1018  // Yes, Iterate through the vector
1019 
1020  // Variable
1021  // The iterator to the start:
1022  auto startIter = startVertices_.begin();
1023  // The end point of the vector to consider. We will delete by swapping elements to the end, moving this
1024  // iterator towards the start, and then erasing once at the end.
1025  auto startEnd = startVertices_.end();
1026 
1027  // Run until at the end:
1028  while (startIter != startEnd)
1029  {
1030  // Check if this start has met the criteria to be pruned
1031  if (this->canVertexBeDisconnected(*startIter))
1032  {
1033  // It has, remove the start vertex DO NOT consider it as a sample. It is marked as a root node,
1034  // so having it as a sample would cause all kinds of problems, also it shouldn't be possible for
1035  // it to ever be useful as a sample anyway, unless there is a very weird cost function in play.
1036  numPruned.second = numPruned.second + this->removeFromVertices(*startIter, false);
1037 
1038  // Count as a disconnected vertex
1039  ++numPruned.first;
1040 
1041  // Disconnect from parent if necessary, cascading cost updates.
1042  if ((*startIter)->hasParent())
1043  {
1044  this->removeEdgeBetweenVertexAndParent(*startIter, true);
1045  queuePtr_->removeOutEdgesConnectedToVertexFromQueue(*startIter);
1046  }
1047 
1048  // // Remove it from the vertex queue.
1049  // queuePtr_->unqueueVertex(*startIter);
1050 
1051  // Store the start vertex in the pruned vector, in case it later needs to be readded:
1052  prunedStartVertices_.push_back(*startIter);
1053 
1054  // Remove this start from the vector.
1055  // Swap it to the element before our *new* end
1056  if (startIter != (startEnd - 1))
1057  {
1058  using std::swap; // Enable Koenig Lookup.
1059  swap(*startIter, *(startEnd - 1));
1060  }
1061 
1062  // Move the end forward by one, marking it to be deleted
1063  --startEnd;
1064 
1065  // Leave the iterator where it is, as we need to recheck this element that we pulled from the
1066  // back
1067  }
1068  else
1069  {
1070  // Still valid, move to the next one:
1071  ++startIter;
1072  }
1073  }
1074 
1075  // Erase any elements moved to the "new end" of the vector
1076  if (startEnd != startVertices_.end())
1077  {
1078  startVertices_.erase(startEnd, startVertices_.end());
1079  }
1080  // No else, nothing to delete
1081  }
1082  // No else, can't prune 1 start.
1083 
1084  // Are there superfluous goals to prune?
1085  if (goalVertices_.size() > 1u)
1086  {
1087  // Yes, Iterate through the vector
1088 
1089  // Variable
1090  // The iterator to the start:
1091  auto goalIter = goalVertices_.begin();
1092  // The end point of the vector to consider. We will delete by swapping elements to the end, moving this
1093  // iterator towards the start, and then erasing once at the end.
1094  auto goalEnd = goalVertices_.end();
1095 
1096  // Run until at the end:
1097  while (goalIter != goalEnd)
1098  {
1099  // Check if this goal has met the criteria to be pruned, but make sure it is not the goal that is
1100  // the current best solution. The current goal can satisfy the condition for pruning if the
1101  // heuristic is perfect, since samples are pruned unless the combined costs of the heuristic
1102  // cost-to-come and heuristic cost-to-go is (strictly) lower than the current solution cost. If the
1103  // heuristic is perfect, then the heuristic cost-to-come is equal to the solution cost (and the
1104  // heuristic cost-to-go is zero, since this is a goal).
1105  if (this->canSampleBePruned(*goalIter) &&
1106  costHelpPtr_->isCostNotEquivalentTo((*goalIter)->getCost(), solutionCost_))
1107  {
1108  // It has, remove the goal vertex completely
1109  // Check if this vertex is in the tree
1110  if ((*goalIter)->isInTree())
1111  {
1112  // Disconnect from parent if necessary, cascading cost updates.
1113  if ((*goalIter)->hasParent())
1114  {
1115  this->removeEdgeBetweenVertexAndParent(*goalIter, true);
1116  queuePtr_->removeOutEdgesConnectedToVertexFromQueue(*goalIter);
1117 
1118  // If the goal is inconsistent, it needs to be removed from the set of inconsistent
1119  // vertices.
1120  if (!(*goalIter)->isConsistent())
1121  {
1122  queuePtr_->removeFromInconsistentSet(*goalIter);
1123  }
1124  }
1125 
1126  // Remove it from the set of vertices, recycling if necessary.
1127  this->removeFromVertices(*goalIter, true);
1128 
1129  // and as a vertex, allowing it to move to the set of samples.
1130  numPruned.second = numPruned.second + this->removeFromVertices(*goalIter, true);
1131 
1132  // Count it as a disconnected vertex
1133  ++numPruned.first;
1134  }
1135  else
1136  {
1137  // It is not, so just it like a sample
1138  this->pruneSample(*goalIter);
1139 
1140  // Count a pruned sample
1141  ++numPruned.second;
1142  }
1143 
1144  // Store the start vertex in the pruned vector, in case it later needs to be readded:
1145  prunedGoalVertices_.push_back(*goalIter);
1146 
1147  // Remove this goal from the vector.
1148  // Swap it to the element before our *new* end
1149  if (goalIter != (goalEnd - 1))
1150  {
1151  std::swap(*goalIter, *(goalEnd - 1));
1152  }
1153 
1154  // Move the end forward by one, marking it to be deleted
1155  --goalEnd;
1156 
1157  // Leave the iterator where it is, as we need to recheck this element that we pulled from the
1158  // back
1159  }
1160  else
1161  {
1162  // The goal is still valid, get the next
1163  ++goalIter;
1164  }
1165  }
1166 
1167  // Erase any elements moved to the "new end" of the vector
1168  if (goalEnd != goalVertices_.end())
1169  {
1170  goalVertices_.erase(goalEnd, goalVertices_.end());
1171  }
1172  // No else, nothing to delete
1173  }
1174  // No else, can't prune 1 goal.
1175 
1176  // We don't need to update our approximate solution (if we're providing one) as we will only prune once a
1177  // solution exists.
1178 
1179  // Return the amount of work done
1180  return numPruned;
1181  }
1182 
1183  std::pair<unsigned int, unsigned int> BITstar::ImplicitGraph::pruneSamples()
1184  {
1185  // The number of samples pruned in this pass:
1186  std::pair<unsigned int, unsigned int> numPruned{0u, 0u};
1187 
1188  // Get the vector of samples.
1189  VertexPtrVector samples;
1190  samples_->list(samples);
1191 
1192  // Are we dropping samples anytime we prune?
1193  if (dropSamplesOnPrune_)
1194  {
1195  std::size_t numFreeStatesPruned = 0u;
1196  for (const auto &sample : samples)
1197  {
1198  if (!sample->isInTree())
1199  {
1200  this->pruneSample(sample);
1201  ++numPruned.second;
1202  ++numFreeStatesPruned;
1203  }
1204  }
1205 
1206  // Store the number of uniform samples.
1207  numUniformStates_ = 0u;
1208 
1209  // Increase the global counter.
1210  numFreeStatesPruned_ += numFreeStatesPruned;
1211  }
1212  else
1213  {
1214  // Iterate through the vector and remove any samples that should not be in the graph.
1215  for (const auto &sample : samples)
1216  {
1217  if (sample->isInTree())
1218  {
1219  if (this->canVertexBeDisconnected(sample))
1220  {
1221  numPruned = numPruned + this->pruneVertex(sample);
1222  }
1223  }
1224  // Check if this state should be pruned.
1225  else if (this->canSampleBePruned(sample))
1226  {
1227  // It should, remove the sample from the NN structure.
1228  this->pruneSample(sample);
1229 
1230  // Keep track of how many are pruned.
1231  ++numPruned.second;
1232  }
1233  // No else, keep sample.
1234  }
1235  }
1236 
1237  return numPruned;
1238  }
1239 
1241  {
1242  ASSERT_SETUP
1243 
1244  // Threshold should always be g_t(x_g)
1245 
1246  // Prune the vertex if it could cannot part of a better solution in the current graph. Greater-than just in
1247  // case we're using an edge that is exactly optimally connected.
1248  // g_t(v) + h^(v) > g_t(x_g)?
1249  return costHelpPtr_->isCostWorseThan(costHelpPtr_->currentHeuristicVertex(vertex), solutionCost_);
1250  }
1251 
1253  {
1254  ASSERT_SETUP
1255 
1256 #ifdef BITSTAR_DEBUG
1257  if (sample->isPruned())
1258  {
1259  throw ompl::Exception("Asking whether a pruned sample can be pruned.");
1260  }
1261 #endif // BITSTAR_DEBUG
1262 
1263  // Threshold should always be g_t(x_g)
1264  // Prune the unconnected sample if it could never be better of a better solution.
1265  // g^(v) + h^(v) >= g_t(x_g)?
1266  return costHelpPtr_->isCostWorseThanOrEquivalentTo(costHelpPtr_->lowerBoundHeuristicVertex(sample),
1267  solutionCost_);
1268  }
1269 
1270  void BITstar::ImplicitGraph::testClosestToGoal(const VertexConstPtr &vertex)
1271  {
1272  // Find the shortest distance between the given vertex and a goal
1273  double distance = std::numeric_limits<double>::max();
1274  problemDefinition_->getGoal()->isSatisfied(vertex->state(), &distance);
1275 
1276  // Compare to the current best approximate solution
1277  if (distance < closestDistanceToGoal_)
1278  {
1279  // Better, update the approximate solution.
1280  closestVertexToGoal_ = vertex;
1281  closestDistanceToGoal_ = distance;
1282  }
1283  // No else, don't update if worse.
1284  }
1285 
1286  ompl::base::Cost BITstar::ImplicitGraph::calculateNeighbourhoodCost(const VertexConstPtr &vertex) const
1287  {
1288 #ifdef BITSTAR_DEBUG
1289  if (vertex->isPruned())
1290  {
1291  throw ompl::Exception("Calculating the neighbourhood cost of a pruned vertex.");
1292  }
1293 #endif // BITSTAR_DEBUG
1294  // Are we using JIT sampling?
1295  if (useJustInTimeSampling_)
1296  {
1297  // We are, return the maximum heuristic cost that represents a sample in the neighbourhood of the given
1298  // vertex.
1299  // There is no point generating samples worse the best solution (maxCost_) even if those samples are in
1300  // this vertex's neighbourhood.
1301  return costHelpPtr_->betterCost(
1302  solutionCost_, costHelpPtr_->combineCosts(costHelpPtr_->lowerBoundHeuristicVertex(vertex),
1303  ompl::base::Cost(2.0 * r_)));
1304  }
1305 
1306  // We are not, return the maximum cost we'd ever want to sample
1307  return solutionCost_;
1308  }
1309 
1310  void BITstar::ImplicitGraph::updateNearestTerms()
1311  {
1312  // The number of uniformly distributed states.
1313  unsigned int numUniformStates = 0u;
1314 
1315  // Calculate N, are we dropping samples?
1316  if (dropSamplesOnPrune_)
1317  {
1318  // We are, so we've been tracking the number of uniform states, just use that
1319  numUniformStates = numUniformStates_;
1320  }
1321  else if (isPruningEnabled_)
1322  {
1323  // We are not dropping samples but pruning is enabled, then all samples are uniform.
1324  numUniformStates = samples_->size();
1325  }
1326  else
1327  {
1328  // We don't prune, so we have to check how many samples are in the informed set.
1329  numUniformStates = computeNumberOfSamplesInInformedSet();
1330  }
1331 
1332  // If this is the first batch, we will be calculating the connection limits from only the starts and goals
1333  // for an RGG with m samples. That will be a complex graph. In this case, let us calculate the connection
1334  // limits considering the samples about to be generated. Doing so is equivalent to setting an upper-bound on
1335  // the radius, which RRT* does with it's min(maxEdgeLength, RGG-radius).
1336  if (numUniformStates == (startVertices_.size() + goalVertices_.size()))
1337  {
1338  numUniformStates = numUniformStates + numNewSamplesInCurrentBatch_;
1339  }
1340 
1341  // Now update the appropriate term
1342  if (useKNearest_)
1343  {
1344  k_ = this->calculateK(numUniformStates);
1345  }
1346  else
1347  {
1348  r_ = this->calculateR(numUniformStates);
1349  }
1350  }
1351 
1352  std::size_t BITstar::ImplicitGraph::computeNumberOfSamplesInInformedSet() const
1353  {
1354  // Get the samples as a vector.
1355  std::vector<VertexPtr> samples;
1356  samples_->list(samples);
1357 
1358  // Return the number of samples that can not be pruned.
1359  return std::count_if(samples.begin(), samples.end(),
1360  [this](const VertexPtr &sample) { return !canSampleBePruned(sample); });
1361  }
1362 
1363  double BITstar::ImplicitGraph::calculateR(unsigned int numUniformSamples) const
1364  {
1365  // Cast to double for readability. (?)
1366  auto stateDimension = static_cast<double>(spaceInformation_->getStateDimension());
1367  auto graphCardinality = static_cast<double>(numUniformSamples);
1368 
1369  // Calculate the term and return.
1370  return rewireFactor_ * this->calculateMinimumRggR() *
1371  std::pow(std::log(graphCardinality) / graphCardinality, 1.0 / stateDimension);
1372  }
1373 
1374  unsigned int BITstar::ImplicitGraph::calculateK(unsigned int numUniformSamples) const
1375  {
1376  // Calculate the term and return
1377  return std::ceil(rewireFactor_ * k_rgg_ * std::log(static_cast<double>(numUniformSamples)));
1378  }
1379 
1380  double BITstar::ImplicitGraph::calculateMinimumRggR() const
1381  {
1382  // Variables
1383  // The dimension cast as a double for readibility;
1384  auto stateDimension = static_cast<double>(spaceInformation_->getStateDimension());
1385 
1386  // Calculate the term and return
1387  // RRT*
1388  return std::pow(2.0 * (1.0 + 1.0 / stateDimension) *
1389  (approximationMeasure_ / unitNBallMeasure(spaceInformation_->getStateDimension())),
1390  1.0 / stateDimension);
1391 
1392  // Relevant work on calculating the minimum radius:
1393  /*
1394  // PRM*,RRG radius (biggest for unit-volume problem)
1395  // See Theorem 34 in Karaman & Frazzoli IJRR 2011
1396  return 2.0 * std::pow((1.0 + 1.0 / dimDbl) *
1397  (approximationMeasure_ /
1398  unitNBallMeasure(si_->getStateDimension())),
1399  1.0 / dimDbl);
1400 
1401  // FMT* radius (R2: smallest, R3: equiv to RRT*, Higher d: middle).
1402  // See Theorem 4.1 in Janson et al. IJRR 2015
1403  return 2.0 * std::pow((1.0 / dimDbl) *
1404  (approximationMeasure_ /
1405  unitNBallMeasure(si_->getStateDimension())),
1406  1.0 / dimDbl);
1407 
1408  // RRT* radius (smallest for unit-volume problems above R3).
1409  // See Theorem 38 in Karaman & Frazzoli IJRR 2011
1410  return std::pow(2.0 * (1.0 + 1.0 / dimDbl) *
1411  (approximationMeasure_ /
1412  unitNBallMeasure(si_->getStateDimension())),
1413  1.0 / dimDbl);
1414  */
1415  }
1416 
1417  double BITstar::ImplicitGraph::calculateMinimumRggK() const
1418  {
1419  // The dimension cast as a double for readibility.
1420  auto stateDimension = static_cast<double>(spaceInformation_->getStateDimension());
1421 
1422  // Calculate the term and return.
1423  return boost::math::constants::e<double>() +
1424  (boost::math::constants::e<double>() / stateDimension); // RRG k-nearest
1425  }
1426 
1427  void BITstar::ImplicitGraph::assertSetup() const
1428  {
1429  if (!isSetup_)
1430  {
1431  throw ompl::Exception("BITstar::ImplicitGraph was used before it was setup.");
1432  }
1433  }
1435 
1437  // Boring sets/gets (Public):
1439  {
1440  return (!startVertices_.empty());
1441  }
1442 
1444  {
1445  return (!goalVertices_.empty());
1446  }
1447 
1448  BITstar::VertexPtrVector::const_iterator BITstar::ImplicitGraph::startVerticesBeginConst() const
1449  {
1450  return startVertices_.cbegin();
1451  }
1452 
1453  BITstar::VertexPtrVector::const_iterator BITstar::ImplicitGraph::startVerticesEndConst() const
1454  {
1455  return startVertices_.cend();
1456  }
1457 
1458  BITstar::VertexPtrVector::const_iterator BITstar::ImplicitGraph::goalVerticesBeginConst() const
1459  {
1460  return goalVertices_.cbegin();
1461  }
1462 
1463  BITstar::VertexPtrVector::const_iterator BITstar::ImplicitGraph::goalVerticesEndConst() const
1464  {
1465  return goalVertices_.cend();
1466  }
1467 
1469  {
1470  return minCost_;
1471  }
1472 
1474  {
1475  return sampler_->hasInformedMeasure();
1476  }
1477 
1479  {
1480  return sampler_->getInformedMeasure(cost);
1481  }
1482 
1484  {
1485 #ifdef BITSTAR_DEBUG
1486  if (!findApprox_)
1487  {
1488  throw ompl::Exception("Approximate solutions are not being tracked.");
1489  }
1490 #endif // BITSTAR_DEBUG
1491  return closestVertexToGoal_;
1492  }
1493 
1495  {
1496 #ifdef BITSTAR_DEBUG
1497  if (!findApprox_)
1498  {
1499  throw ompl::Exception("Approximate solutions are not being tracked.");
1500  }
1501 #endif // BITSTAR_DEBUG
1502  return closestDistanceToGoal_;
1503  }
1504 
1506  {
1507 #ifdef BITSTAR_DEBUG
1508  if (!useKNearest_)
1509  {
1510  throw ompl::Exception("Using an r-disc graph.");
1511  }
1512 #endif // BITSTAR_DEBUG
1513  return k_;
1514  }
1515 
1517  {
1518 #ifdef BITSTAR_DEBUG
1519  if (useKNearest_)
1520  {
1521  throw ompl::Exception("Using a k-nearest graph.");
1522  }
1523 #endif // BITSTAR_DEBUG
1524  return r_;
1525  }
1526 
1528  {
1529  VertexPtrVector samples;
1530  samples_->list(samples);
1531  return samples;
1532  }
1533 
1535  {
1536  // Store
1537  rewireFactor_ = rewireFactor;
1538 
1539  // Check if there's things to update
1540  if (isSetup_)
1541  {
1542  // Reinitialize the terms:
1543  this->updateNearestTerms();
1544  }
1545  }
1546 
1548  {
1549  return rewireFactor_;
1550  }
1551 
1553  {
1554  // Assure that we're not trying to enable k-nearest with JIT sampling already on
1555  if (useKNearest && useJustInTimeSampling_)
1556  {
1557  OMPL_WARN("%s (ImplicitGraph): The k-nearest variant of BIT* cannot be used with JIT sampling, "
1558  "continuing to use the r-disc variant.",
1559  nameFunc_().c_str());
1560  }
1561  else
1562  {
1563  // Store
1564  useKNearest_ = useKNearest;
1565 
1566  // Check if there's things to update
1567  if (isSetup_)
1568  {
1569  // Calculate the current term:
1570  this->updateNearestTerms();
1571  }
1572  }
1573  }
1574 
1576  {
1577  return useKNearest_;
1578  }
1579 
1581  {
1582  // Assure that we're not trying to enable k-nearest with JIT sampling already on
1583  if (useKNearest_ && useJit)
1584  {
1585  OMPL_WARN("%s (ImplicitGraph): Just-in-time sampling cannot be used with the k-nearest variant of "
1586  "BIT*, continuing to use regular sampling.",
1587  nameFunc_().c_str());
1588  }
1589  else
1590  {
1591  // Store
1592  useJustInTimeSampling_ = useJit;
1593 
1594  // Announce limitation:
1595  if (useJit)
1596  {
1597  OMPL_INFORM("%s (ImplicitGraph): Just-in-time sampling is currently only implemented for problems "
1598  "seeking to minimize path-length.",
1599  nameFunc_().c_str());
1600  }
1601  // No else
1602  }
1603  }
1604 
1606  {
1607  return useJustInTimeSampling_;
1608  }
1609 
1611  {
1612  // Make sure we're not already setup
1613  if (isSetup_)
1614  {
1615  OMPL_WARN("%s (ImplicitGraph): Periodic sample removal cannot be changed once BIT* is setup. "
1616  "Continuing to use the previous setting.",
1617  nameFunc_().c_str());
1618  }
1619  else
1620  {
1621  // Store
1622  dropSamplesOnPrune_ = dropSamples;
1623  }
1624  }
1625 
1627  {
1628  isPruningEnabled_ = usePruning;
1629  }
1630 
1632  {
1633  return dropSamplesOnPrune_;
1634  }
1635 
1637  {
1638  // Is the flag changing?
1639  if (findApproximate != findApprox_)
1640  {
1641  // Store the flag
1642  findApprox_ = findApproximate;
1643 
1644  // Check if we are enabling or disabling approximate solution support
1645  if (!findApprox_)
1646  {
1647  // We're turning it off, clear the approximate solution variables:
1648  closestDistanceToGoal_ = std::numeric_limits<double>::infinity();
1649  closestVertexToGoal_.reset();
1650  }
1651  else
1652  {
1653  // We are turning it on, do we have an exact solution?
1654  if (!hasExactSolution_)
1655  {
1656  // We don't, find our current best approximate solution:
1657  this->updateVertexClosestToGoal();
1658  }
1659  // No else, exact is better than approximate.
1660  }
1661  }
1662  // No else, no change.
1663  }
1664 
1666  {
1667  return findApprox_;
1668  }
1669 
1671  {
1672  averageNumOfAllowedFailedAttemptsWhenSampling_ = number;
1673  }
1674 
1676  {
1677  return averageNumOfAllowedFailedAttemptsWhenSampling_;
1678  }
1679 
1680  template <template <typename T> class NN>
1682  {
1683  // Check if the problem is already setup, if so, the NN structs have data in them and you can't really
1684  // change them:
1685  if (isSetup_)
1686  {
1687  OMPL_WARN("%s (ImplicitGraph): The nearest neighbour datastructures cannot be changed once the problem "
1688  "is setup. Continuing to use the existing containers.",
1689  nameFunc_().c_str());
1690  }
1691  else
1692  {
1693  // The problem isn't setup yet, create NN structs of the specified type:
1694  samples_ = std::make_shared<NN<VertexPtr>>();
1695  }
1696  }
1697 
1699  {
1700  return samples_->size();
1701  }
1702 
1704  {
1705  VertexPtrVector samples;
1706  samples_->list(samples);
1707  return std::count_if(samples.begin(), samples.end(),
1708  [](const VertexPtr &sample) { return sample->isInTree(); });
1709  }
1710 
1712  {
1713  return numSamples_;
1714  }
1715 
1717  {
1718  return numVertices_;
1719  }
1720 
1722  {
1723  return numFreeStatesPruned_;
1724  }
1725 
1727  {
1728  return numVerticesDisconnected_;
1729  }
1730 
1732  {
1733  return numNearestNeighbours_;
1734  }
1735 
1737  {
1738  return numStateCollisionChecks_;
1739  }
1741  } // namespace geometric
1742 } // 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:88
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.
Definition: AppBase.h:21
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 ...