ompl/datastructures/NearestNeighborsFLANN.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: Mark Moll */ 00036 00037 #ifndef OMPL_DATASTRUCTURES_NEAREST_NEIGHBORS_FLANN_ 00038 #define OMPL_DATASTRUCTURES_NEAREST_NEIGHBORS_FLANN_ 00039 00040 #include "ompl/config.h" 00041 #if OMPL_HAVE_FLANN == 0 00042 # error FLANN is not available. Please use a different NearestNeighbors data structure. 00043 #else 00044 00045 #include "ompl/datastructures/NearestNeighbors.h" 00046 #include "ompl/base/StateSpace.h" 00047 00048 #include <flann/flann.hpp> 00049 00050 namespace ompl 00051 { 00055 template<typename _T> 00056 class FLANNDistance 00057 { 00058 public: 00059 typedef _T ElementType; 00060 typedef double ResultType; 00061 00062 FLANNDistance(const typename NearestNeighbors<_T>::DistanceFunction& distFun) 00063 : distFun_(distFun) 00064 { 00065 } 00066 00067 template <typename Iterator1, typename Iterator2> 00068 ResultType operator()(Iterator1 a, Iterator2 b, 00069 size_t /*size*/, ResultType /*worst_dist*/ = -1) const 00070 { 00071 return distFun_(*a, *b); 00072 } 00073 protected: 00074 const typename NearestNeighbors<_T>::DistanceFunction& distFun_; 00075 }; 00076 00086 template<typename _T, typename _Dist = FLANNDistance<_T> > 00087 class NearestNeighborsFLANN : public NearestNeighbors<_T> 00088 { 00089 public: 00090 00091 NearestNeighborsFLANN(const boost::shared_ptr<flann::IndexParams> ¶ms) 00092 : index_(0), params_(params), searchParams_(32, 0., true), dimension_(1) 00093 { 00094 } 00095 00096 virtual ~NearestNeighborsFLANN() 00097 { 00098 if (index_) 00099 delete index_; 00100 } 00101 00102 virtual void clear() 00103 { 00104 if (index_) 00105 { 00106 delete index_; 00107 index_ = NULL; 00108 } 00109 data_.clear(); 00110 } 00111 00112 virtual bool reportsSortedResults() const 00113 { 00114 return searchParams_.sorted; 00115 } 00116 00117 virtual void setDistanceFunction(const typename NearestNeighbors<_T>::DistanceFunction &distFun) 00118 { 00119 NearestNeighbors<_T>::setDistanceFunction(distFun); 00120 rebuildIndex(); 00121 } 00122 00123 virtual void add(const _T &data) 00124 { 00125 bool rebuild = index_ && (data_.size() + 1 > data_.capacity()); 00126 00127 if (rebuild) 00128 rebuildIndex(2 * data_.capacity()); 00129 00130 data_.push_back(data); 00131 const flann::Matrix<_T> mat(&data_.back(), 1, dimension_); 00132 00133 if (index_) 00134 index_->addPoints(mat, std::numeric_limits<float>::max()/size()); 00135 else 00136 createIndex(mat); 00137 } 00138 virtual void add(const std::vector<_T> &data) 00139 { 00140 unsigned int oldSize = data_.size(); 00141 unsigned int newSize = oldSize + data.size(); 00142 bool rebuild = index_ && (newSize > data_.capacity()); 00143 00144 if (rebuild) 00145 rebuildIndex(std::max(2 * oldSize, newSize)); 00146 00147 if (index_) 00148 { 00149 std::copy(data.begin(), data.end(), data_.begin() + oldSize); 00150 const flann::Matrix<_T> mat(&data_[oldSize], data.size(), dimension_); 00151 index_->addPoints(mat, std::numeric_limits<float>::max()/size()); 00152 } 00153 else 00154 { 00155 data_ = data; 00156 const flann::Matrix<_T> mat(&data_[0], data_.size(), dimension_); 00157 createIndex(mat); 00158 } 00159 } 00160 virtual bool remove(const _T& data) 00161 { 00162 if (!index_) return false; 00163 _T& elt = const_cast<_T&>(data); 00164 const flann::Matrix<_T> query(&elt, 1, dimension_); 00165 std::vector<std::vector<size_t> > indices(1); 00166 std::vector<std::vector<double> > dists(1); 00167 index_->knnSearch(query, indices, dists, 1, searchParams_); 00168 if (*index_->getPoint(indices[0][0]) == data) 00169 { 00170 index_->removePoint(indices[0][0]); 00171 rebuildIndex(); 00172 return true; 00173 } 00174 return false; 00175 } 00176 virtual _T nearest(const _T &data) const 00177 { 00178 if (size()) 00179 { 00180 _T& elt = const_cast<_T&>(data); 00181 const flann::Matrix<_T> query(&elt, 1, dimension_); 00182 std::vector<std::vector<size_t> > indices(1); 00183 std::vector<std::vector<double> > dists(1); 00184 index_->knnSearch(query, indices, dists, 1, searchParams_); 00185 return *index_->getPoint(indices[0][0]); 00186 } 00187 throw Exception("No elements found in nearest neighbors data structure"); 00188 } 00191 virtual void nearestK(const _T &data, std::size_t k, std::vector<_T> &nbh) const 00192 { 00193 _T& elt = const_cast<_T&>(data); 00194 const flann::Matrix<_T> query(&elt, 1, dimension_); 00195 std::vector<std::vector<size_t> > indices; 00196 std::vector<std::vector<double> > dists; 00197 k = index_ ? index_->knnSearch(query, indices, dists, k, searchParams_) : 0; 00198 nbh.resize(k); 00199 for (std::size_t i = 0 ; i < k ; ++i) 00200 nbh[i] = *index_->getPoint(indices[0][i]); 00201 } 00204 virtual void nearestR(const _T &data, double radius, std::vector<_T> &nbh) const 00205 { 00206 _T& elt = const_cast<_T&>(data); 00207 flann::Matrix<_T> query(&elt, 1, dimension_); 00208 std::vector<std::vector<size_t> > indices; 00209 std::vector<std::vector<double> > dists; 00210 int k = index_ ? index_->radiusSearch(query, indices, dists, radius, searchParams_) : 0; 00211 nbh.resize(k); 00212 for (int i = 0 ; i < k ; ++i) 00213 nbh[i] = *index_->getPoint(indices[0][i]); 00214 } 00215 00216 virtual std::size_t size() const 00217 { 00218 return index_ ? index_->size() : 0; 00219 } 00220 00221 virtual void list(std::vector<_T> &data) const 00222 { 00223 std::size_t sz = size(); 00224 if (sz == 0) 00225 { 00226 data.resize(0); 00227 return; 00228 } 00229 const _T& dummy = *index_->getPoint(0); 00230 int checks = searchParams_.checks; 00231 searchParams_.checks = size(); 00232 nearestK(dummy, sz, data); 00233 searchParams_.checks = checks; 00234 } 00235 00240 virtual void setIndexParams(const boost::shared_ptr<flann::IndexParams> ¶ms) 00241 { 00242 params_ = params; 00243 rebuildIndex(); 00244 } 00245 00247 virtual const boost::shared_ptr<flann::IndexParams>& getIndexParams() const 00248 { 00249 return params_; 00250 } 00251 00254 virtual void setSearchParams(const flann::SearchParams& searchParams) 00255 { 00256 searchParams_ = searchParams; 00257 } 00258 00261 flann::SearchParams& getSearchParams() 00262 { 00263 return searchParams_; 00264 } 00265 00268 const flann::SearchParams& getSearchParams() const 00269 { 00270 return searchParams_; 00271 } 00272 00273 unsigned int getContainerSize() const 00274 { 00275 return dimension_; 00276 } 00277 00278 protected: 00279 00282 void createIndex(const flann::Matrix<_T>& mat) 00283 { 00284 index_ = new flann::Index<_Dist>(mat, *params_, _Dist(NearestNeighbors<_T>::distFun_)); 00285 index_->buildIndex(); 00286 } 00287 00290 void rebuildIndex(unsigned int capacity = 0) 00291 { 00292 if (index_) 00293 { 00294 std::vector<_T> data; 00295 list(data); 00296 clear(); 00297 if (capacity) 00298 data_.reserve(capacity); 00299 add(data); 00300 } 00301 } 00302 00305 std::vector<_T> data_; 00306 00308 flann::Index<_Dist>* index_; 00309 00312 boost::shared_ptr<flann::IndexParams> params_; 00313 00315 mutable flann::SearchParams searchParams_; 00316 00320 unsigned int dimension_; 00321 }; 00322 00323 template<> 00324 void NearestNeighborsFLANN<double, flann::L2<double> >::createIndex( 00325 const flann::Matrix<double>& mat) 00326 { 00327 index_ = new flann::Index<flann::L2<double> >(mat, *params_); 00328 index_->buildIndex(); 00329 } 00330 00331 template<typename _T, typename _Dist = FLANNDistance<_T> > 00332 class NearestNeighborsFLANNLinear : public NearestNeighborsFLANN<_T, _Dist> 00333 { 00334 public: 00335 NearestNeighborsFLANNLinear() 00336 : NearestNeighborsFLANN<_T, _Dist>( 00337 boost::shared_ptr<flann::LinearIndexParams>( 00338 new flann::LinearIndexParams())) 00339 { 00340 } 00341 }; 00342 00343 template<typename _T, typename _Dist = FLANNDistance<_T> > 00344 class NearestNeighborsFLANNHierarchicalClustering : public NearestNeighborsFLANN<_T, _Dist> 00345 { 00346 public: 00347 NearestNeighborsFLANNHierarchicalClustering() 00348 : NearestNeighborsFLANN<_T, _Dist>( 00349 boost::shared_ptr<flann::HierarchicalClusteringIndexParams>( 00350 new flann::HierarchicalClusteringIndexParams())) 00351 { 00352 } 00353 }; 00354 00355 } 00356 #endif 00357 00358 #endif