ConnectionStrategy.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Rice University
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 Rice University 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 /* Author: James D. Marble */
36 
37 #ifndef OMPL_GEOMETRIC_PLANNERS_PRM_CONNECTION_STRATEGY_
38 #define OMPL_GEOMETRIC_PLANNERS_PRM_CONNECTION_STRATEGY_
39 
40 #include "ompl/datastructures/NearestNeighbors.h"
41 #include <functional>
42 #include <memory>
43 #include <boost/math/constants/constants.hpp>
44 #include <algorithm>
45 #include <utility>
46 #include <vector>
47 
48 namespace ompl
49 {
50  namespace geometric
51  {
55  template <class Milestone>
56  class KStrategy
57  {
58  public:
61  KStrategy(const unsigned int k, std::shared_ptr<NearestNeighbors<Milestone>> nn) : k_(k), nn_(std::move(nn))
62  {
63  neighbors_.reserve(k_);
64  }
65 
66  virtual ~KStrategy() = default;
67 
69  void setNearestNeighbors(const std::shared_ptr<NearestNeighbors<Milestone>> &nn)
70  {
71  nn_ = nn;
72  }
73 
77  const std::vector<Milestone> &operator()(const Milestone &m)
78  {
79  nn_->nearestK(m, k_, neighbors_);
80  return neighbors_;
81  }
82 
83  protected:
85  unsigned int k_;
86 
88  std::shared_ptr<NearestNeighbors<Milestone>> nn_;
89 
91  std::vector<Milestone> neighbors_;
92  };
93 
119  template <class Milestone>
120  class KStarStrategy : public KStrategy<Milestone>
121  {
122  public:
123  typedef std::function<unsigned int()> NumNeighborsFn;
133  KStarStrategy(const NumNeighborsFn &n, const std::shared_ptr<NearestNeighbors<Milestone>> &nn,
134  const unsigned int d = 1)
135  : KStrategy<Milestone>(n(), nn)
136  , n_(n)
137  , kPRMConstant_(boost::math::constants::e<double>() + (boost::math::constants::e<double>() / (double)d))
138  {
139  }
140 
141  const std::vector<Milestone> &operator()(const Milestone &m)
142  {
143  KStrategy<Milestone>::k_ = static_cast<unsigned int>(ceil(kPRMConstant_ * log((double)n_())));
144  return static_cast<KStrategy<Milestone> &>(*this)(m);
145  }
146 
147  protected:
149  const NumNeighborsFn n_;
150  const double kPRMConstant_;
151  };
152 
156  template <class Milestone>
157  class KBoundedStrategy : public KStrategy<Milestone>
158  {
159  public:
167  KBoundedStrategy(const unsigned int k, const double bound,
168  const std::shared_ptr<NearestNeighbors<Milestone>> &nn)
169  : KStrategy<Milestone>(k, nn), bound_(bound)
170  {
171  }
172 
173  const std::vector<Milestone> &operator()(const Milestone &m)
174  {
175  std::vector<Milestone> &result = KStrategy<Milestone>::neighbors_;
177  if (result.empty())
178  return result;
179  const typename NearestNeighbors<Milestone>::DistanceFunction &dist =
180  KStrategy<Milestone>::nn_->getDistanceFunction();
181  if (!KStrategy<Milestone>::nn_->reportsSortedResults())
182  std::sort(result.begin(), result.end(), dist);
183  std::size_t newCount = result.size();
184  while (newCount > 0 && dist(result[newCount - 1], m) > bound_)
185  --newCount;
186  result.resize(newCount);
187  return result;
188  }
189 
190  protected:
192  const double bound_;
193  };
194  }
195 }
196 
197 #endif
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
std::vector< Milestone > neighbors_
Scratch space for storing k-nearest neighbors.
STL namespace.
const double bound_
The maximum distance at which nearby milestones are reported.
KBoundedStrategy(const unsigned int k, const double bound, const std::shared_ptr< NearestNeighbors< Milestone >> &nn)
Constructor.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
unsigned int k_
Maximum number of nearest neighbors to attempt to connect new milestones to.
Return at most k neighbors, as long as they are also within a specified bound.
const std::vector< Milestone > & operator()(const Milestone &m)
Given a milestone m, find the number of nearest neighbors connection attempts that should be made fro...
const NumNeighborsFn n_
Function returning the number of milestones added to the roadmap so far.
KStarStrategy(const NumNeighborsFn &n, const std::shared_ptr< NearestNeighbors< Milestone >> &nn, const unsigned int d=1)
Constructor.
KStrategy(const unsigned int k, std::shared_ptr< NearestNeighbors< Milestone >> nn)
Constructor takes the maximum number of nearest neighbors to return (k) and the nearest neighbors dat...
Abstract representation of a container that can perform nearest neighbors queries.
void setNearestNeighbors(const std::shared_ptr< NearestNeighbors< Milestone >> &nn)
Set the nearest neighbors datastructure to use.
Make the minimal number of connections required to ensure asymptotic optimality.
std::shared_ptr< NearestNeighbors< Milestone > > nn_
Nearest neighbors data structure.
std::function< double(const _T &, const _T &)> DistanceFunction
The definition of a distance function.