flann_search.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010-2011, Willow Garage, Inc.
 *
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * $Id: flann_search.h 5170 2012-03-18 04:21:56Z rusu $
 */

#ifndef PCL_SEARCH_FLANN_SEARCH_H_
#define PCL_SEARCH_FLANN_SEARCH_H_

#include <pcl/search/search.h>
#include <pcl/common/time.h>
#include <pcl/point_representation.h>

namespace flann
{
  template<typename T> class NNIndex;
  template<typename T> class L2;
  template<typename T> class L2_Simple;
  template<typename T> class Matrix;
}

namespace pcl
{
  namespace search
  {

    template<typename PointT, typename FlannDistance=flann::L2_Simple <float> >
    class FlannSearch: public Search<PointT>
    {
      typedef typename Search<PointT>::PointCloud PointCloud;
      typedef typename Search<PointT>::PointCloudConstPtr PointCloudConstPtr;

      typedef boost::shared_ptr<std::vector<int> > IndicesPtr;
      typedef boost::shared_ptr<const std::vector<int> > IndicesConstPtr;
      typedef flann::NNIndex< FlannDistance > Index;
      typedef boost::shared_ptr<flann::NNIndex <FlannDistance > > IndexPtr;
      typedef boost::shared_ptr<flann::Matrix <float> > MatrixPtr;
      typedef boost::shared_ptr<const flann::Matrix <float> > MatrixConstPtr;

      typedef pcl::PointRepresentation<PointT> PointRepresentation;
      //typedef boost::shared_ptr<PointRepresentation> PointRepresentationPtr;
      typedef boost::shared_ptr<const PointRepresentation> PointRepresentationConstPtr;

      using Search<PointT>::input_;
      using Search<PointT>::indices_;
      using Search<PointT>::sorted_results_;

      public:
        typedef boost::shared_ptr<FlannSearch<PointT> > Ptr;
        typedef boost::shared_ptr<const FlannSearch<PointT> > ConstPtr;

        class FlannIndexCreator
        {
          public:
            virtual IndexPtr createIndex (MatrixConstPtr data)=0;
        };

        class KdTreeIndexCreator: public FlannIndexCreator
        {
          public:
            KdTreeIndexCreator (unsigned int max_leaf_size=15) : max_leaf_size_ (max_leaf_size){}
            virtual IndexPtr createIndex (MatrixConstPtr data);
          private:
            unsigned int max_leaf_size_;
        };

        FlannSearch (bool sorted = true, FlannIndexCreator* creator = new KdTreeIndexCreator());

        virtual
        ~FlannSearch ();


        //void
        //setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices = IndicesConstPtr ());

        inline void
        setEpsilon (double eps)
        {
          eps_ = eps;
        }

        inline double
        getEpsilon ()
        {
          return (eps_);
        }

        inline virtual void
        setInputCloud (const PointCloudConstPtr& cloud, const IndicesConstPtr& indices = IndicesConstPtr ());

        int
        nearestKSearch (const PointT &point, int k, std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const;


        virtual void
        nearestKSearch (const PointCloud& cloud, const std::vector<int>& indices, int k, 
                        std::vector< std::vector<int> >& k_indices, std::vector< std::vector<float> >& k_sqr_distances) const;

        int
        radiusSearch (const PointT& point, double radius, 
                      std::vector<int> &k_indices, std::vector<float> &k_sqr_distances,
                      unsigned int max_nn = 0) const;

        virtual void
        radiusSearch (const PointCloud& cloud, const std::vector<int>& indices, double radius, std::vector< std::vector<int> >& k_indices,
                std::vector< std::vector<float> >& k_sqr_distances, unsigned int max_nn=0) const;

        inline void
        setPointRepresentation (const PointRepresentationConstPtr &point_representation)
        {
          point_representation_ = point_representation;
          setInputCloud (input_, indices_);  // re-create the tree, since point_represenation might change things such as the scaling of the point clouds.
        }

        inline PointRepresentationConstPtr const
        getPointRepresentation ()
        {
          return (point_representation_);
        }

      protected:

        void convertInputToFlannMatrix();

        IndexPtr index_;

        FlannIndexCreator *creator_;

        MatrixPtr input_flann_;

        float eps_;
        bool input_copied_for_flann_;

        PointRepresentationConstPtr point_representation_;

        int dim_;

        std::vector<int> index_mapping_;
        bool identity_mapping_;

    };
  }
}

#define PCL_INSTANTIATE_FlannSearch(T) template class PCL_EXPORTS pcl::search::FlannSearch<T>;

#endif    // PCL_SEARCH_KDTREE_H_