brute_force.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  */
00037 
00038 #ifndef PCL_SEARCH_IMPL_BRUTE_FORCE_SEARCH_H_
00039 #define PCL_SEARCH_IMPL_BRUTE_FORCE_SEARCH_H_
00040 
00041 #include <pcl/search/brute_force.h>
00042 #include <queue>
00043 
00045 template <typename PointT> float
00046 pcl::search::BruteForce<PointT>::getDistSqr (
00047     const PointT& point1, const PointT& point2) const
00048 {
00049   return (point1.getVector3fMap () - point2.getVector3fMap ()).squaredNorm ();
00050 }
00051 
00053 template <typename PointT> int
00054 pcl::search::BruteForce<PointT>::nearestKSearch (
00055     const PointT& point, int k, std::vector<int>& k_indices, std::vector<float>& k_distances) const
00056 {
00057   assert (isFinite (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
00058   
00059   k_indices.clear ();
00060   k_distances.clear ();
00061   if (k < 1)
00062     return 0;
00063 
00064   if (input_->is_dense)
00065     return denseKSearch (point, k, k_indices, k_distances);
00066   else
00067     return sparseKSearch (point, k, k_indices, k_distances);
00068 }
00069 
00071 template <typename PointT> int
00072 pcl::search::BruteForce<PointT>::denseKSearch (
00073     const PointT &point, int k, std::vector<int> &k_indices, std::vector<float> &k_distances) const
00074 {
00075   // container for first k elements -> O(1) for insertion, since order not required here
00076   std::vector<Entry> result;
00077   result.reserve (k);
00078   std::priority_queue<Entry> queue;
00079   if (indices_ != NULL)
00080   {
00081     std::vector<int>::const_iterator iIt =indices_->begin ();
00082     std::vector<int>::const_iterator iEnd = indices_->begin () + std::min (static_cast<unsigned> (k), static_cast<unsigned> (indices_->size ()));
00083     for (; iIt != iEnd; ++iIt)
00084       result.push_back (Entry (*iIt, getDistSqr (input_->points[*iIt], point)));
00085 
00086     queue = std::priority_queue<Entry> (result.begin (), result.end ());
00087 
00088     // add the rest
00089     Entry entry;
00090     for (; iIt != indices_->end (); ++iIt)
00091     {
00092       entry.distance = getDistSqr (input_->points[*iIt], point);
00093       if (queue.top ().distance > entry.distance)
00094       {
00095         entry.index = *iIt;
00096         queue.pop ();
00097         queue.push (entry);
00098       }
00099     }
00100   }
00101   else
00102   {
00103     Entry entry;
00104     for (entry.index = 0; entry.index < std::min (static_cast<unsigned> (k), static_cast<unsigned> (input_->size ())); ++entry.index)
00105     {
00106       entry.distance = getDistSqr (input_->points[entry.index], point);
00107       result.push_back (entry);
00108     }
00109 
00110     queue = std::priority_queue<Entry> (result.begin (), result.end ());
00111 
00112     // add the rest
00113     for (; entry.index < input_->size (); ++entry.index)
00114     {
00115       entry.distance = getDistSqr (input_->points[entry.index], point);
00116       if (queue.top ().distance > entry.distance)
00117       {
00118         queue.pop ();
00119         queue.push (entry);
00120       }      
00121     }
00122   }
00123 
00124   k_indices.resize (queue.size ());
00125   k_distances.resize (queue.size ());
00126   size_t idx = queue.size () - 1;
00127   while (!queue.empty ())
00128   {
00129     k_indices [idx] = queue.top ().index;
00130     k_distances [idx] = queue.top ().distance;
00131     queue.pop ();
00132     --idx;
00133   }
00134   
00135   return (static_cast<int> (k_indices.size ()));
00136 }
00137 
00139 template <typename PointT> int
00140 pcl::search::BruteForce<PointT>::sparseKSearch (
00141     const PointT &point, int k, std::vector<int> &k_indices, std::vector<float> &k_distances) const
00142 {
00143   // result used to collect the first k neighbors -> unordered
00144   std::vector<Entry> result;
00145   result.reserve (k);
00146   
00147   std::priority_queue<Entry> queue;
00148   if (indices_ != NULL)
00149   {
00150     std::vector<int>::const_iterator iIt =indices_->begin ();
00151     for (; iIt != indices_->end () && result.size () < static_cast<unsigned> (k); ++iIt)
00152     {
00153       if (pcl_isfinite (input_->points[*iIt].x))
00154         result.push_back (Entry (*iIt, getDistSqr (input_->points[*iIt], point)));
00155     }
00156     
00157     queue = std::priority_queue<Entry> (result.begin (), result.end ());
00158 
00159     // either we have k elements, or there are none left to iterate >in either case we're fine
00160     // add the rest
00161     Entry entry;
00162     for (; iIt != indices_->end (); ++iIt)
00163     {
00164       if (!pcl_isfinite (input_->points[*iIt].x))
00165         continue;
00166 
00167       entry.distance = getDistSqr (input_->points[*iIt], point);
00168       if (queue.top ().distance > entry.distance)
00169       {
00170         entry.index = *iIt;
00171         queue.pop ();
00172         queue.push (entry);
00173       }
00174     }
00175   }
00176   else
00177   {
00178     Entry entry;
00179     for (entry.index = 0; entry.index < input_->size () && result.size () < static_cast<unsigned> (k); ++entry.index)
00180     {
00181       if (pcl_isfinite (input_->points[entry.index].x))
00182       {
00183         entry.distance = getDistSqr (input_->points[entry.index], point);
00184         result.push_back (entry);
00185       }
00186     }
00187     queue = std::priority_queue<Entry> (result.begin (), result.end ());
00188     
00189     // add the rest
00190     for (; entry.index < input_->size (); ++entry.index)
00191     {
00192       if (!pcl_isfinite (input_->points[entry.index].x))
00193         continue;
00194 
00195       entry.distance = getDistSqr (input_->points[entry.index], point);
00196       if (queue.top ().distance > entry.distance)
00197       {
00198         queue.pop ();
00199         queue.push (entry);
00200       }
00201     }
00202   }
00203   
00204   k_indices.resize (queue.size ());
00205   k_distances.resize (queue.size ());
00206   size_t idx = queue.size () - 1;
00207   while (!queue.empty ())
00208   {
00209     k_indices [idx] = queue.top ().index;
00210     k_distances [idx] = queue.top ().distance;
00211     queue.pop ();
00212     --idx;
00213   }
00214   return (static_cast<int> (k_indices.size ()));
00215 }
00216 
00218 template <typename PointT> int
00219 pcl::search::BruteForce<PointT>::denseRadiusSearch (
00220     const PointT& point, double radius,
00221     std::vector<int> &k_indices, std::vector<float> &k_sqr_distances,
00222     unsigned int max_nn) const
00223 {  
00224   radius *= radius;
00225 
00226   size_t reserve = max_nn;
00227   if (reserve == 0)
00228   {
00229     if (indices_ != NULL)
00230       reserve = std::min (indices_->size (), input_->size ());
00231     else
00232       reserve = input_->size ();
00233   }
00234   k_indices.reserve (reserve);
00235   k_sqr_distances.reserve (reserve);
00236   float distance;
00237   if (indices_ != NULL)
00238   {
00239     for (std::vector<int>::const_iterator iIt =indices_->begin (); iIt != indices_->end (); ++iIt)
00240     {
00241       distance = getDistSqr (input_->points[*iIt], point);
00242       if (distance <= radius)
00243       {
00244         k_indices.push_back (*iIt);
00245         k_sqr_distances.push_back (distance);
00246         if (k_indices.size () == max_nn) // max_nn = 0 -> never true
00247           break;
00248       }
00249     }
00250   }
00251   else
00252   {
00253     for (unsigned index = 0; index < input_->size (); ++index)
00254     {
00255       distance = getDistSqr (input_->points[index], point);
00256       if (distance <= radius)
00257       {
00258         k_indices.push_back (index);
00259         k_sqr_distances.push_back (distance);
00260         if (k_indices.size () == max_nn) // never true if max_nn = 0
00261           break;
00262       }
00263     }
00264   }
00265 
00266   if (sorted_results_)
00267     this->sortResults (k_indices, k_sqr_distances);
00268   
00269   return (static_cast<int> (k_indices.size ()));
00270 }
00271 
00273 template <typename PointT> int
00274 pcl::search::BruteForce<PointT>::sparseRadiusSearch (
00275     const PointT& point, double radius,
00276     std::vector<int> &k_indices, std::vector<float> &k_sqr_distances,
00277     unsigned int max_nn) const
00278 {
00279   radius *= radius;
00280 
00281   size_t reserve = max_nn;
00282   if (reserve == 0)
00283   {
00284     if (indices_ != NULL)
00285       reserve = std::min (indices_->size (), input_->size ());
00286     else
00287       reserve = input_->size ();
00288   }
00289   k_indices.reserve (reserve);
00290   k_sqr_distances.reserve (reserve);
00291 
00292   float distance;
00293   if (indices_ != NULL)
00294   {
00295     for (std::vector<int>::const_iterator iIt =indices_->begin (); iIt != indices_->end (); ++iIt)
00296     {
00297       if (!pcl_isfinite (input_->points[*iIt].x))
00298         continue;
00299 
00300       distance = getDistSqr (input_->points[*iIt], point);
00301       if (distance <= radius)
00302       {
00303         k_indices.push_back (*iIt);
00304         k_sqr_distances.push_back (distance);
00305         if (k_indices.size () == max_nn) // never true if max_nn = 0
00306           break;
00307       }
00308     }
00309   }
00310   else
00311   {
00312     for (unsigned index = 0; index < input_->size (); ++index)
00313     {
00314       if (!pcl_isfinite (input_->points[index].x))
00315         continue;
00316       distance = getDistSqr (input_->points[index], point);
00317       if (distance <= radius)
00318       {
00319         k_indices.push_back (index);
00320         k_sqr_distances.push_back (distance);
00321         if (k_indices.size () == max_nn) // never true if max_nn = 0
00322           break;
00323       }
00324     }
00325   }
00326 
00327   if (sorted_results_)
00328     this->sortResults (k_indices, k_sqr_distances);
00329 
00330   return (static_cast<int> (k_indices.size ()));
00331 }
00332 
00334 template <typename PointT> int
00335 pcl::search::BruteForce<PointT>::radiusSearch (
00336     const PointT& point, double radius, std::vector<int> &k_indices,
00337     std::vector<float> &k_sqr_distances, unsigned int max_nn) const
00338 {
00339   assert (isFinite (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
00340   
00341   k_indices.clear ();
00342   k_sqr_distances.clear ();
00343   if (radius <= 0)
00344     return 0;
00345 
00346   if (input_->is_dense)
00347     return denseRadiusSearch (point, radius, k_indices, k_sqr_distances, max_nn);
00348   else
00349     return sparseRadiusSearch (point, radius, k_indices, k_sqr_distances, max_nn);
00350 }
00351 
00352 #define PCL_INSTANTIATE_BruteForce(T) template class PCL_EXPORTS pcl::search::BruteForce<T>;
00353 
00354 #endif //PCL_SEARCH_IMPL_BRUTE_FORCE_SEARCH_H_


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:14:40