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 <boost/function.hpp>
42 #include <boost/shared_ptr.hpp>
43 #include <boost/math/constants/constants.hpp>
44 #include <algorithm>
45 #include <vector>
46 
47 namespace ompl
48 {
49 
50  namespace geometric
51  {
52 
56  template <class Milestone>
57  class KStrategy
58  {
59  public:
60 
63  KStrategy(const unsigned int k,
64  const boost::shared_ptr< NearestNeighbors<Milestone> > &nn) :
65  k_(k), nn_(nn)
66  {
67  neighbors_.reserve(k_);
68  }
69 
70  virtual ~KStrategy()
71  {
72  }
73 
75  void setNearestNeighbors(const boost::shared_ptr< NearestNeighbors<Milestone> > &nn)
76  {
77  nn_ = nn;
78  }
79 
83  const std::vector<Milestone>& operator()(const Milestone &m)
84  {
85  nn_->nearestK(m, k_, neighbors_);
86  return neighbors_;
87  }
88 
89  protected:
90 
92  unsigned int k_;
93 
95  boost::shared_ptr< NearestNeighbors<Milestone> > nn_;
96 
98  std::vector<Milestone> neighbors_;
99  };
100 
126  template <class Milestone>
127  class KStarStrategy : public KStrategy<Milestone>
128  {
129  public:
130  typedef boost::function<unsigned int()> NumNeighborsFn;
140  KStarStrategy(const NumNeighborsFn& n,
141  const boost::shared_ptr< NearestNeighbors<Milestone> > &nn,
142  const unsigned int d = 1) :
143  KStrategy<Milestone>(n(), nn), n_(n),
144  kPRMConstant_(boost::math::constants::e<double>() + (boost::math::constants::e<double>() / (double)d))
145  {
146  }
147 
148  const std::vector<Milestone>& operator()(const Milestone &m)
149  {
150  KStrategy<Milestone>::k_ = static_cast<unsigned int>(ceil(kPRMConstant_ * log((double)n_())));
151  return static_cast<KStrategy<Milestone>&>(*this)(m);
152  }
153 
154  protected:
155 
157  const NumNeighborsFn n_;
158  const double kPRMConstant_;
159 
160  };
161 
162 
166  template <class Milestone>
167  class KBoundedStrategy : public KStrategy<Milestone>
168  {
169  public:
170 
178  KBoundedStrategy(const unsigned int k,
179  const double bound,
180  const boost::shared_ptr< NearestNeighbors<Milestone> > &nn) :
181  KStrategy<Milestone>(k, nn), bound_(bound)
182  {
183  }
184 
185  const std::vector<Milestone>& operator()(const Milestone &m)
186  {
187  std::vector<Milestone> &result = KStrategy<Milestone>::neighbors_;
189  if (result.empty()) return result;
190  const typename NearestNeighbors<Milestone>::DistanceFunction &dist =
191  KStrategy<Milestone>::nn_->getDistanceFunction();
192  if (!KStrategy<Milestone>::nn_->reportsSortedResults())
193  std::sort(result.begin(), result.end(), dist);
194  std::size_t newCount = result.size();
195  while (newCount > 0 && dist(result[newCount - 1], m) > bound_) --newCount;
196  result.resize(newCount);
197  return result;
198  }
199 
200  protected:
201 
203  const double bound_;
204 
205  };
206 
207  }
208 }
209 
210 #endif
KStarStrategy(const NumNeighborsFn &n, const boost::shared_ptr< NearestNeighbors< Milestone > > &nn, const unsigned int d=1)
Constructor.
boost::shared_ptr< NearestNeighbors< Milestone > > nn_
Nearest neighbors data structure.
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:104
boost::function< double(const _T &, const _T &)> DistanceFunction
The definition of a distance function.
std::vector< Milestone > neighbors_
Scratch space for storing k-nearest neighbors.
const double bound_
The maximum distance at which nearby milestones are reported.
Main namespace. Contains everything in this library.
Definition: Cost.h:42
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...
KBoundedStrategy(const unsigned int k, const double bound, const boost::shared_ptr< NearestNeighbors< Milestone > > &nn)
Constructor.
const NumNeighborsFn n_
Function returning the number of milestones added to the roadmap so far.
void setNearestNeighbors(const boost::shared_ptr< NearestNeighbors< Milestone > > &nn)
Set the nearest neighbors datastructure to use.
KStrategy(const unsigned int k, const boost::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.
Make the minimal number of connections required to ensure asymptotic optimality.