flann_search.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2011, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id: flann_search.hpp 5170 2012-03-18 04:21:56Z rusu $
00037  *
00038  */
00039 
00040 #ifndef PCL_SEARCH_IMPL_FLANN_SEARCH_H_
00041 #define PCL_SEARCH_IMPL_FLANN_SEARCH_H_
00042 
00043 #include <pcl/search/flann_search.h>
00044 #include <pcl/kdtree/flann.h>
00045 
00047 template <typename PointT, typename FlannDistance>
00048 typename pcl::search::FlannSearch<PointT, FlannDistance>::IndexPtr
00049 pcl::search::FlannSearch<PointT, FlannDistance>::KdTreeIndexCreator::createIndex (MatrixConstPtr data)
00050 {
00051   return (IndexPtr (new flann::KDTreeSingleIndex<FlannDistance> (*data,flann::KDTreeSingleIndexParams (max_leaf_size_))));
00052 }
00053 
00054 
00056 template <typename PointT, typename FlannDistance>
00057 pcl::search::FlannSearch<PointT, FlannDistance>::FlannSearch(bool sorted, FlannIndexCreator *creator) : pcl::search::Search<PointT> ("FlannSearch",sorted),
00058   creator_ (creator), eps_ (0), input_copied_for_flann_ (false)
00059 {
00060   point_representation_.reset (new DefaultPointRepresentation<PointT>);
00061   dim_ = point_representation_->getNumberOfDimensions ();
00062 }
00063 
00065 template <typename PointT, typename FlannDistance>
00066 pcl::search::FlannSearch<PointT, FlannDistance>::~FlannSearch()
00067 {
00068   delete creator_;
00069   if (input_copied_for_flann_)
00070     delete [] input_flann_->ptr();
00071 }
00072 
00074 template <typename PointT, typename FlannDistance> void
00075 pcl::search::FlannSearch<PointT, FlannDistance>::setInputCloud (const PointCloudConstPtr& cloud, const IndicesConstPtr& indices)
00076 {
00077   input_ = cloud;
00078   indices_ = indices;
00079   convertInputToFlannMatrix ();
00080   index_ = creator_->createIndex (input_flann_);
00081   index_->buildIndex ();
00082 }
00083 
00085 template <typename PointT, typename FlannDistance> int
00086 pcl::search::FlannSearch<PointT, FlannDistance>::nearestKSearch (const PointT &point, int k, std::vector<int> &indices, std::vector<float> &dists) const
00087 {
00088   assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); // remove this check as soon as FLANN does NaN checks internally
00089   bool can_cast = point_representation_->isTrivial ();
00090 
00091   float* data = 0;
00092   if (!can_cast)
00093   {
00094     data = new float [point_representation_->getNumberOfDimensions ()];
00095     point_representation_->vectorize (point,data);
00096   }
00097 
00098   float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&point)): data;
00099   const flann::Matrix<float> m (cdata ,1, point_representation_->getNumberOfDimensions ());
00100 
00101   flann::SearchParams p(-1);
00102   p.eps = eps_;
00103   p.sorted = sorted_results_;
00104   if (indices.size() != static_cast<unsigned int> (k))
00105     indices.resize (k,-1);
00106   if (dists.size() != static_cast<unsigned int> (k))
00107     dists.resize (k);
00108   flann::Matrix<int> i (&indices[0],1,k);
00109   flann::Matrix<float> d (&dists[0],1,k);
00110   int result = index_->knnSearch (m,i,d,k, p);
00111 
00112   delete [] data;
00113 
00114   if (!identity_mapping_)
00115   {
00116     for (size_t i = 0; i < static_cast<unsigned int> (k); ++i)
00117     {
00118       int& neighbor_index = indices[i];
00119       neighbor_index = index_mapping_[neighbor_index];
00120     }
00121   }
00122   return result;
00123 }
00124 
00126 template <typename PointT, typename FlannDistance> void
00127 pcl::search::FlannSearch<PointT, FlannDistance>::nearestKSearch (
00128     const PointCloud& cloud, const std::vector<int>& indices, int k, std::vector< std::vector<int> >& k_indices,
00129     std::vector< std::vector<float> >& k_sqr_distances) const
00130 {
00131   if (indices.empty ())
00132   {
00133     k_indices.resize (cloud.size ());
00134     k_sqr_distances.resize (cloud.size ());
00135 
00136     if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally
00137     {
00138       for (size_t i = 0; i < cloud.size(); i++)
00139       {
00140         assert (point_representation_->isValid (cloud[i]) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
00141       }
00142     }
00143 
00144     bool can_cast = point_representation_->isTrivial ();
00145 
00146     // full point cloud + trivial copy operation = no need to do any conversion/copying to the flann matrix!
00147     float* data=0;
00148     if (!can_cast)
00149     {
00150       data = new float[dim_*cloud.size ()];
00151       for (size_t i = 0; i < cloud.size (); ++i)
00152       {
00153         float* out = data+i*dim_;
00154         point_representation_->vectorize (cloud[i],out);
00155       }
00156     }
00157 
00158     // const cast is evil, but the matrix constructor won't change the data, and the
00159     // search won't change the matrix
00160     float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&cloud[0])): data;
00161     const flann::Matrix<float> m (cdata ,cloud.size (), dim_, can_cast ? sizeof (PointT) : dim_ * sizeof (float) );
00162 
00163     flann::SearchParams p;
00164     p.sorted = sorted_results_;
00165     p.eps = eps_;
00166     index_->knnSearch (m,k_indices,k_sqr_distances,k, p);
00167 
00168     delete [] data;
00169   }
00170   else // if indices are present, the cloud has to be copied anyway. Only copy the relevant parts of the points here.
00171   {
00172     k_indices.resize (indices.size ());
00173     k_sqr_distances.resize (indices.size ());
00174 
00175     if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally
00176     {
00177       for (size_t i = 0; i < indices.size(); i++)
00178       {
00179         assert (point_representation_->isValid (cloud [indices[i]]) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
00180       }
00181     }
00182 
00183     float* data=new float [dim_*indices.size ()];
00184     for (size_t i = 0; i < indices.size (); ++i)
00185     {
00186       float* out = data+i*dim_;
00187       point_representation_->vectorize (cloud[indices[i]],out);
00188     }
00189     const flann::Matrix<float> m (data ,indices.size (), point_representation_->getNumberOfDimensions ());
00190 
00191     flann::SearchParams p;
00192     p.sorted = sorted_results_;
00193     p.eps = eps_;
00194     index_->knnSearch (m,k_indices,k_sqr_distances,k, p);
00195 
00196     delete[] data;
00197   }
00198   if (!identity_mapping_)
00199   {
00200     for (size_t j = 0; j < k_indices.size (); ++j)
00201     {
00202       for (size_t i = 0; i < static_cast<unsigned int> (k); ++i)
00203       {
00204         int& neighbor_index = k_indices[j][i];
00205         neighbor_index = index_mapping_[neighbor_index];
00206       }
00207     }
00208   }
00209 }
00210 
00212 template <typename PointT, typename FlannDistance> int
00213 pcl::search::FlannSearch<PointT, FlannDistance>::radiusSearch (const PointT& point, double radius,
00214     std::vector<int> &indices, std::vector<float> &distances,
00215     unsigned int max_nn) const
00216 {
00217   assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!"); // remove this check as soon as FLANN does NaN checks internally
00218   bool can_cast = point_representation_->isTrivial ();
00219 
00220   float* data = 0;
00221   if (!can_cast)
00222   {
00223     data = new float [point_representation_->getNumberOfDimensions ()];
00224     point_representation_->vectorize (point,data);
00225   }
00226 
00227   float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&point)) : data;
00228   const flann::Matrix<float> m (cdata ,1, point_representation_->getNumberOfDimensions ());
00229 
00230   flann::SearchParams p;
00231   p.sorted = sorted_results_;
00232   p.eps = eps_;
00233   p.max_neighbors = max_nn > 0 ? max_nn : -1;
00234   std::vector<std::vector<int> > i (1);
00235   std::vector<std::vector<float> > d (1);
00236   int result = index_->radiusSearch (m,i,d,static_cast<float> (radius * radius), p);
00237 
00238   delete [] data;
00239   indices = i [0];
00240   distances = d [0];
00241 
00242   if (!identity_mapping_)
00243   {
00244     for (size_t i = 0; i < indices.size (); ++i)
00245     {
00246       int& neighbor_index = indices [i];
00247       neighbor_index = index_mapping_ [neighbor_index];
00248     }
00249   }
00250   return result;
00251 }
00252 
00254 template <typename PointT, typename FlannDistance> void
00255 pcl::search::FlannSearch<PointT, FlannDistance>::radiusSearch (
00256     const PointCloud& cloud, const std::vector<int>& indices, double radius, std::vector< std::vector<int> >& k_indices,
00257     std::vector< std::vector<float> >& k_sqr_distances, unsigned int max_nn) const
00258 {
00259   if (indices.empty ()) // full point cloud + trivial copy operation = no need to do any conversion/copying to the flann matrix!
00260   {
00261     k_indices.resize (cloud.size ());
00262     k_sqr_distances.resize (cloud.size ());
00263 
00264     if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally
00265     {
00266       for (size_t i = 0; i < cloud.size(); i++)
00267       {
00268         assert (point_representation_->isValid (cloud[i]) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!");
00269       }
00270     }
00271 
00272     bool can_cast = point_representation_->isTrivial ();
00273 
00274     float* data = 0;
00275     if (!can_cast)
00276     {
00277       data = new float[dim_*cloud.size ()];
00278       for (size_t i = 0; i < cloud.size (); ++i)
00279       {
00280         float* out = data+i*dim_;
00281         point_representation_->vectorize (cloud[i],out);
00282       }
00283     }
00284 
00285     float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&cloud[0])) : data;
00286     const flann::Matrix<float> m (cdata ,cloud.size (), dim_, can_cast ? sizeof (PointT) : dim_ * sizeof (float));
00287 
00288     flann::SearchParams p;
00289     p.sorted = sorted_results_;
00290     p.eps = eps_;
00291     // here: max_nn==0: take all neighbors. flann: max_nn==0: return no neighbors, only count them. max_nn==-1: return all neighbors
00292     p.max_neighbors = max_nn > 0 ? max_nn : -1;
00293     index_->radiusSearch (m,k_indices,k_sqr_distances,static_cast<float> (radius * radius), p);
00294 
00295     delete [] data;
00296   }
00297   else // if indices are present, the cloud has to be copied anyway. Only copy the relevant parts of the points here.
00298   {
00299     k_indices.resize (indices.size ());
00300     k_sqr_distances.resize (indices.size ());
00301 
00302     if (! cloud.is_dense)  // remove this check as soon as FLANN does NaN checks internally
00303     {
00304       for (size_t i = 0; i < indices.size(); i++)
00305       {
00306         assert (point_representation_->isValid (cloud [indices[i]]) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!");
00307       }
00308     }
00309 
00310     float* data = new float [dim_ * indices.size ()];
00311     for (size_t i = 0; i < indices.size (); ++i)
00312     {
00313       float* out = data+i*dim_;
00314       point_representation_->vectorize (cloud[indices[i]], out);
00315     }
00316     const flann::Matrix<float> m (data, cloud.size (), point_representation_->getNumberOfDimensions ());
00317 
00318     flann::SearchParams p;
00319     p.sorted = sorted_results_;
00320     p.eps = eps_;
00321     // here: max_nn==0: take all neighbors. flann: max_nn==0: return no neighbors, only count them. max_nn==-1: return all neighbors
00322     p.max_neighbors = max_nn > 0 ? max_nn : -1;
00323     index_->radiusSearch (m, k_indices, k_sqr_distances, static_cast<float> (radius * radius), p);
00324 
00325     delete[] data;
00326   }
00327   if (!identity_mapping_)
00328   {
00329     for (size_t j = 0; j < k_indices.size (); ++j )
00330     {
00331       for (size_t i = 0; i < k_indices[j].size (); ++i)
00332       {
00333         int& neighbor_index = k_indices[j][i];
00334         neighbor_index = index_mapping_[neighbor_index];
00335       }
00336     }
00337   }
00338 }
00339 
00341 template <typename PointT, typename FlannDistance> void
00342 pcl::search::FlannSearch<PointT, FlannDistance>::convertInputToFlannMatrix ()
00343 {
00344   size_t original_no_of_points = indices_ && !indices_->empty () ? indices_->size () : input_->size ();
00345 
00346   if (input_copied_for_flann_)
00347     delete input_flann_->ptr();
00348   input_copied_for_flann_ = true;
00349   index_mapping_.clear();
00350   identity_mapping_ = true;
00351 
00352   //cloud_ = (float*)malloc (original_no_of_points * dim_ * sizeof (float));
00353   //index_mapping_.reserve(original_no_of_points);
00354   //identity_mapping_ = true;
00355 
00356   if (!indices_ || indices_->empty ())
00357   {
00358     // best case: all points can be passed to flann without any conversions
00359     if (input_->is_dense && point_representation_->isTrivial ())
00360     {
00361       // const cast is evil, but flann won't change the data
00362       input_flann_ = MatrixPtr (new flann::Matrix<float> (const_cast<float*>(reinterpret_cast<const float*>(&(*input_) [0])), original_no_of_points, point_representation_->getNumberOfDimensions (),sizeof (PointT)));
00363       input_copied_for_flann_ = false;
00364     }
00365     else
00366     {
00367       input_flann_ = MatrixPtr (new flann::Matrix<float> (new float[original_no_of_points*point_representation_->getNumberOfDimensions ()], original_no_of_points, point_representation_->getNumberOfDimensions ()));
00368       float* cloud_ptr = input_flann_->ptr();
00369       for (size_t i = 0; i < original_no_of_points; ++i)
00370       {
00371         const PointT& point = (*input_)[i];
00372         // Check if the point is invalid
00373         if (!point_representation_->isValid (point))
00374         {
00375           identity_mapping_ = false;
00376           continue;
00377         }
00378 
00379         index_mapping_.push_back (static_cast<int> (i));  // If the returned index should be for the indices vector
00380 
00381         point_representation_->vectorize (point, cloud_ptr);
00382         cloud_ptr += dim_;
00383       }
00384     }
00385 
00386   }
00387   else
00388   {
00389     input_flann_ = MatrixPtr (new flann::Matrix<float> (new float[original_no_of_points*point_representation_->getNumberOfDimensions ()], original_no_of_points, point_representation_->getNumberOfDimensions ()));
00390     float* cloud_ptr = input_flann_->ptr();
00391     for (size_t indices_index = 0; indices_index < original_no_of_points; ++indices_index)
00392     {
00393       int cloud_index = (*indices_)[indices_index];
00394       const PointT&  point = (*input_)[cloud_index];
00395       // Check if the point is invalid
00396       if (!point_representation_->isValid (point))
00397       {
00398         identity_mapping_ = false;
00399         continue;
00400       }
00401 
00402       index_mapping_.push_back (static_cast<int> (indices_index));  // If the returned index should be for the indices vector
00403 
00404       point_representation_->vectorize (point, cloud_ptr);
00405       cloud_ptr += dim_;
00406     }
00407   }
00408   if (input_copied_for_flann_)
00409     input_flann_->rows = index_mapping_.size ();
00410 }
00411 
00412 #define PCL_INSTANTIATE_FlannSearch(T) template class PCL_EXPORTS pcl::search::FlannSearch<T>;
00413 
00414 #endif


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:15:11