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> &params)
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> &params)
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
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines