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  unsigned int getNumNeighbors() const
84  {
85  return k_;
86  }
87  protected:
89  unsigned int k_;
90 
92  std::shared_ptr<NearestNeighbors<Milestone>> nn_;
93 
95  std::vector<Milestone> neighbors_;
96  };
97 
123  template <class Milestone>
124  class KStarStrategy : public KStrategy<Milestone>
125  {
126  public:
127  using NumNeighborsFn = std::function<unsigned int()>;
137  KStarStrategy(const NumNeighborsFn &n, const std::shared_ptr<NearestNeighbors<Milestone>> &nn,
138  const unsigned int d = 1)
139  : KStrategy<Milestone>(n(), nn)
140  , n_(n)
141  , kPRMConstant_(boost::math::constants::e<double>() + (boost::math::constants::e<double>() / (double)d))
142  {
143  }
144 
145  const std::vector<Milestone> &operator()(const Milestone &m)
146  {
147  KStrategy<Milestone>::k_ = static_cast<unsigned int>(ceil(kPRMConstant_ * log((double)n_())));
148  return static_cast<KStrategy<Milestone> &>(*this)(m);
149  }
150 
151  protected:
153  const NumNeighborsFn n_;
154  const double kPRMConstant_;
155  };
156 
160  template <class Milestone>
161  class KBoundedStrategy : public KStrategy<Milestone>
162  {
163  public:
171  KBoundedStrategy(const unsigned int k, const double bound,
172  const std::shared_ptr<NearestNeighbors<Milestone>> &nn)
173  : KStrategy<Milestone>(k, nn), bound_(bound)
174  {
175  }
176 
177  const auto &operator()(const Milestone &m)
178  {
179  auto &result = KStrategy<Milestone>::neighbors_;
181  if (result.empty())
182  return result;
183  const auto &dist = KStrategy<Milestone>::nn_->getDistanceFunction();
184  if (!KStrategy<Milestone>::nn_->reportsSortedResults())
185  std::sort(result.begin(), result.end(), dist);
186  auto newCount = result.size();
187  while (newCount > 0 && dist(result[newCount - 1], m) > bound_)
188  --newCount;
189  result.resize(newCount);
190  return result;
191  }
192 
193  protected:
195  const double bound_;
196  };
197  }
198 }
199 
200 #endif
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.
unsigned int k_
Maximum number of nearest neighbors to attempt to connect new milestones to.
void setNearestNeighbors(const std::shared_ptr< NearestNeighbors< Milestone >> &nn)
Set the nearest neighbors datastructure to use.
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...
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::shared_ptr< NearestNeighbors< Milestone > > nn_
Nearest neighbors data structure.
const double bound_
The maximum distance at which nearby milestones are reported.
std::vector< Milestone > neighbors_
Scratch space for storing k-nearest neighbors.
KBoundedStrategy(const unsigned int k, const double bound, const std::shared_ptr< NearestNeighbors< Milestone >> &nn)
Constructor.
Main namespace. Contains everything in this library.
KStarStrategy(const NumNeighborsFn &n, const std::shared_ptr< NearestNeighbors< Milestone >> &nn, const unsigned int d=1)
Constructor.