ompl/geometric/planners/prm/ConnectionStrategy.h
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2011, Rice University 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Rice University nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: James D. Marble */ 00036 00037 #ifndef OMPL_GEOMETRIC_PLANNERS_PRM_CONNECTION_STRATEGY_ 00038 #define OMPL_GEOMETRIC_PLANNERS_PRM_CONNECTION_STRATEGY_ 00039 00040 #include "ompl/datastructures/NearestNeighbors.h" 00041 #include <boost/function.hpp> 00042 #include <boost/shared_ptr.hpp> 00043 #include <boost/math/constants/constants.hpp> 00044 #include <algorithm> 00045 #include <vector> 00046 00047 namespace ompl 00048 { 00049 00050 namespace geometric 00051 { 00052 00056 template <class Milestone> 00057 class KStrategy 00058 { 00059 public: 00060 00063 KStrategy(const unsigned int k, 00064 const boost::shared_ptr< NearestNeighbors<Milestone> > &nn) : 00065 k_(k), nn_(nn) 00066 { 00067 neighbors_.reserve(k_); 00068 } 00069 00070 virtual ~KStrategy() 00071 { 00072 } 00073 00075 void setNearestNeighbors(const boost::shared_ptr< NearestNeighbors<Milestone> > &nn) 00076 { 00077 nn_ = nn; 00078 } 00079 00083 const std::vector<Milestone>& operator()(const Milestone &m) 00084 { 00085 nn_->nearestK(m, k_, neighbors_); 00086 return neighbors_; 00087 } 00088 00089 protected: 00090 00092 unsigned int k_; 00093 00095 boost::shared_ptr< NearestNeighbors<Milestone> > nn_; 00096 00098 std::vector<Milestone> neighbors_; 00099 }; 00100 00126 template <class Milestone> 00127 class KStarStrategy : public KStrategy<Milestone> 00128 { 00129 public: 00130 typedef boost::function<unsigned int()> NumNeighborsFn; 00140 KStarStrategy(const NumNeighborsFn& n, 00141 const boost::shared_ptr< NearestNeighbors<Milestone> > &nn, 00142 const unsigned int d = 1) : 00143 KStrategy<Milestone>(n(), nn), n_(n), 00144 kPRMConstant_(boost::math::constants::e<double>() + (boost::math::constants::e<double>() / (double)d)) 00145 { 00146 } 00147 00148 const std::vector<Milestone>& operator()(const Milestone &m) 00149 { 00150 KStrategy<Milestone>::k_ = static_cast<unsigned int>(ceil(kPRMConstant_ * log((double)n_()))); 00151 return static_cast<KStrategy<Milestone>&>(*this)(m); 00152 } 00153 00154 protected: 00155 00157 const NumNeighborsFn n_; 00158 const double kPRMConstant_; 00159 00160 }; 00161 00162 00166 template <class Milestone> 00167 class KBoundedStrategy : public KStrategy<Milestone> 00168 { 00169 public: 00170 00178 KBoundedStrategy(const unsigned int k, 00179 const double bound, 00180 const boost::shared_ptr< NearestNeighbors<Milestone> > &nn) : 00181 KStrategy<Milestone>(k, nn), bound_(bound) 00182 { 00183 } 00184 00185 const std::vector<Milestone>& operator()(const Milestone &m) 00186 { 00187 std::vector<Milestone> &result = KStrategy<Milestone>::neighbors_; 00188 KStrategy<Milestone>::nn_->nearestK(m, KStrategy<Milestone>::k_, result); 00189 if (result.empty()) return result; 00190 const typename NearestNeighbors<Milestone>::DistanceFunction &dist = 00191 KStrategy<Milestone>::nn_->getDistanceFunction(); 00192 if (!KStrategy<Milestone>::nn_->reportsSortedResults()) 00193 std::sort(result.begin(), result.end(), dist); 00194 std::size_t newCount = result.size(); 00195 while (newCount > 0 && dist(result[newCount - 1], m) > bound_) --newCount; 00196 result.resize(newCount); 00197 return result; 00198 } 00199 00200 protected: 00201 00203 const double bound_; 00204 00205 }; 00206 00207 } 00208 } 00209 00210 #endif