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
48namespace ompl
49{
50 namespace geometric
51 {
55 template <class Milestone>
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 {
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
Return at most k neighbors, as long as they are also within a specified bound.
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.
Make the minimal number of connections required to ensure asymptotic optimality.
KStarStrategy(const NumNeighborsFn &n, const std::shared_ptr< NearestNeighbors< Milestone > > &nn, const unsigned int d=1)
Constructor.
const NumNeighborsFn n_
Function returning the number of milestones added to the roadmap so far.
void setNearestNeighbors(const std::shared_ptr< NearestNeighbors< Milestone > > &nn)
Set the nearest neighbors datastructure to use.
std::shared_ptr< NearestNeighbors< Milestone > > nn_
Nearest neighbors data structure.
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...
unsigned int k_
Maximum number of nearest neighbors to attempt to connect new milestones to.
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...
std::vector< Milestone > neighbors_
Scratch space for storing k-nearest neighbors.
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
Main namespace. Contains everything in this library.
STL namespace.