37 #ifndef OMPL_GEOMETRIC_PLANNERS_PRM_CONNECTION_STRATEGY_
38 #define OMPL_GEOMETRIC_PLANNERS_PRM_CONNECTION_STRATEGY_
40 #include "ompl/datastructures/NearestNeighbors.h"
41 #include <boost/function.hpp>
42 #include <boost/shared_ptr.hpp>
43 #include <boost/math/constants/constants.hpp>
55 template <
class Milestone>
94 boost::shared_ptr< NearestNeighbors<Milestone> >
nn_;
125 template <
class Milestone>
129 typedef boost::function<unsigned int()> NumNeighborsFn;
141 const unsigned int d = 1) :
143 kPRMConstant_(boost::math::constants::e<double>() + (boost::math::constants::e<double>()/(double)d))
147 std::vector<Milestone>& operator()(
const Milestone& m)
156 const NumNeighborsFn
n_;
157 const double kPRMConstant_;
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...
std::vector< Milestone > neighbors_
Scratch space for storing k-nearest neighbors.
unsigned int k_
Maximum number of nearest neighbors to attempt to connect new milestones to.
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.
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.