Point Cloud Library (PCL)  1.3.1
kdtree_flann.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
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 Willow Garage, Inc. 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 
00036 #ifndef PCL_KDTREE_KDTREE_IMPL_FLANN_H_
00037 #define PCL_KDTREE_KDTREE_IMPL_FLANN_H_
00038 
00039 #include "pcl/kdtree/kdtree_flann.h"
00040 #include <pcl/console/print.h>
00041 #include <flann/flann.hpp>
00042 
00044 template <typename PointT, typename Dist> void 
00045 pcl::KdTreeFLANN<PointT, Dist>::setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices)
00046 {
00047   cleanup ();   // Perform an automatic cleanup of structures
00048 
00049   if (!initParameters())
00050     return;
00051 
00052   input_   = cloud;
00053   indices_ = indices;
00054   
00055   if (input_ == NULL)
00056     return;
00057 
00058   m_lock_.lock ();
00059   // Allocate enough data
00060   if (!input_)
00061   {
00062     PCL_ERROR ("[pcl::KdTreeANN::setInputCloud] Invalid input!\n");
00063     return;
00064   }
00065   if (indices != NULL)
00066     convertCloudToArray (*input_, *indices_);
00067   else
00068     convertCloudToArray (*input_);
00069 
00070   initData ();
00071   m_lock_.unlock ();
00072 }
00073 
00075 template <typename PointT, typename Dist> int 
00076 pcl::KdTreeFLANN<PointT, Dist>::nearestKSearch (const PointT &point, int k, 
00077                                                 std::vector<int> &k_indices, 
00078                                                 std::vector<float> &k_distances)
00079 {
00080   if (!point_representation_->isValid (point))
00081   {
00082     //PCL_ERROR_STREAM ("[pcl::KdTreeFLANN::nearestKSearch] Invalid query point given!" << point);
00083     return (0);
00084   }
00085 
00086   if (k_indices.size () < (size_t)k)
00087     k_indices.resize (k);
00088   if (k_distances.size () < (size_t)k) 
00089     k_distances.resize (k);
00090 
00091   std::vector<float> tmp (dim_);
00092   point_representation_->vectorize ((PointT)point, tmp);
00093 
00094   flann::Matrix<int> k_indices_mat (&k_indices[0], 1, k);
00095   flann::Matrix<float> k_distances_mat (&k_distances[0], 1, k);
00096   flann_index_->knnSearch (flann::Matrix<float>(&tmp[0], 1, dim_), k_indices_mat, k_distances_mat, k, flann::SearchParams (-1 ,epsilon_));
00097 
00098   // Do mapping to original point cloud
00099   if (!identity_mapping_) 
00100   {
00101     for (size_t i = 0; i < (size_t)k; ++i)
00102     {
00103       int& neighbor_index = k_indices[i];
00104       neighbor_index = index_mapping_[neighbor_index];
00105     }
00106   }
00107 
00108   return (k);
00109 }
00110 
00112 template <typename PointT, typename Dist> int 
00113 pcl::KdTreeFLANN<PointT, Dist>::radiusSearch (const PointT &point, double radius, std::vector<int> &k_indices,
00114                                               std::vector<float> &k_squared_distances, int max_nn) const
00115 {
00116   static flann::Matrix<int> indices_empty;
00117   static flann::Matrix<float> dists_empty;
00118 
00119   if (!point_representation_->isValid (point))
00120   {
00121     //PCL_ERROR_STREAM ("[pcl::KdTreeFLANN::radiusSearch] Invalid query point given!" << point);
00122     return 0;
00123   }
00124 
00125   std::vector<float> tmp(dim_);
00126   point_representation_->vectorize ((PointT)point, tmp);
00127   radius *= radius; // flann uses squared radius
00128 
00129   size_t size;
00130   if (indices_ == NULL) // no indices set, use full size of point cloud:
00131     size = input_->points.size ();
00132   else
00133     size = indices_->size ();
00134 
00135   int neighbors_in_radius = 0;
00136   if (k_indices.size () == size && k_squared_distances.size () == size)  // preallocated vectors
00137   {
00138     // if using preallocated vectors we ignore max_nn as we are sure to have enought space
00139     // to store all neighbors found in radius
00140     flann::Matrix<int> k_indices_mat (&k_indices[0], 1, k_indices.size());
00141     flann::Matrix<float> k_distances_mat (&k_squared_distances[0], 1, k_squared_distances.size());
00142     neighbors_in_radius = flann_index_->radiusSearch (flann::Matrix<float>(&tmp[0], 1, dim_),
00143         k_indices_mat, k_distances_mat, radius, flann::SearchParams (-1, epsilon_, sorted_));
00144   }
00145   else // need to do search twice, first to find how many neighbors and allocate the vectors
00146   {
00147     neighbors_in_radius = flann_index_->radiusSearch (flann::Matrix<float>(&tmp[0], 1, dim_),
00148         indices_empty, dists_empty, radius, flann::SearchParams (-1, epsilon_, sorted_));
00149     if (max_nn > 0) 
00150     {
00151       neighbors_in_radius = std::min(neighbors_in_radius, max_nn); 
00152     }
00153     k_indices.resize (neighbors_in_radius);
00154     k_squared_distances.resize (neighbors_in_radius);
00155     if (neighbors_in_radius == 0)
00156     {
00157       return (0);
00158     }
00159 
00160     flann::Matrix<int> k_indices_mat (&k_indices[0], 1, k_indices.size());
00161     flann::Matrix<float> k_distances_mat (&k_squared_distances[0], 1, k_squared_distances.size());
00162     flann_index_->radiusSearch (flann::Matrix<float>(&tmp[0], 1, dim_),
00163         k_indices_mat, k_distances_mat, radius, flann::SearchParams (-1, epsilon_, sorted_));
00164 
00165   }
00166 
00167   // Do mapping to original point cloud
00168   if (!identity_mapping_) {
00169     for (int i = 0; i < neighbors_in_radius; ++i)
00170     {
00171       int& neighbor_index = k_indices[i];
00172       neighbor_index = index_mapping_[neighbor_index];
00173     }
00174   }
00175 
00176   return (neighbors_in_radius);
00177 }
00178 
00180 template <typename PointT, typename Dist> void 
00181 pcl::KdTreeFLANN<PointT, Dist>::cleanup ()
00182 {
00183   delete flann_index_;
00184 
00185   m_lock_.lock ();
00186   // Data array cleanup
00187   free (cloud_);
00188   cloud_ = NULL;
00189   index_mapping_.clear();
00190 
00191   if (indices_)
00192     indices_.reset ();
00193 
00194   m_lock_.unlock ();
00195 }
00196 
00198 template <typename PointT, typename Dist> bool 
00199 pcl::KdTreeFLANN<PointT, Dist>::initParameters ()
00200 {
00201   epsilon_ = 0.0;   // default error bound value
00202   dim_ = point_representation_->getNumberOfDimensions (); // Number of dimensions - default is 3 = xyz
00203   // Create the kd_tree representation
00204   return (true);
00205 }
00206 
00208 template <typename PointT, typename Dist> void 
00209 pcl::KdTreeFLANN<PointT, Dist>::initData ()
00210 {
00211   flann_index_ = new FLANNIndex(flann::Matrix<float>(cloud_, index_mapping_.size(), dim_),
00212                           flann::KDTreeSingleIndexParams(15)); // max 15 points/leaf
00213   flann_index_->buildIndex();
00214 }
00215 
00217 template <typename PointT, typename Dist> void 
00218 pcl::KdTreeFLANN<PointT, Dist>::convertCloudToArray (const PointCloud &cloud)
00219 {
00220   // No point in doing anything if the array is empty
00221   if (cloud.points.empty ())
00222   {
00223     cloud_ = NULL;
00224     return;
00225   }
00226 
00227   int original_no_of_points = cloud.points.size();
00228 
00229   cloud_ = (float*)malloc (original_no_of_points * dim_ * sizeof(float));
00230   float* cloud_ptr = cloud_;
00231   index_mapping_.reserve(original_no_of_points);
00232   identity_mapping_ = true;
00233 
00234   for (int cloud_index = 0; cloud_index < original_no_of_points; ++cloud_index)
00235   {
00236     const PointT point = cloud.points[cloud_index];
00237     // Check if the point is invalid
00238     if (!point_representation_->isValid (point))
00239     {
00240       identity_mapping_ = false;
00241       continue;
00242     }
00243 
00244     index_mapping_.push_back(cloud_index);
00245 
00246     point_representation_->vectorize(point, cloud_ptr);
00247     cloud_ptr += dim_;
00248   }
00249 }
00250 
00252 template <typename PointT, typename Dist> void 
00253 pcl::KdTreeFLANN<PointT, Dist>::convertCloudToArray (const PointCloud &cloud, const std::vector<int> &indices)
00254 {
00255   // No point in doing anything if the array is empty
00256   if (cloud.points.empty ())
00257   {
00258     cloud_ = NULL;
00259     return;
00260   }
00261 
00262   int original_no_of_points = indices.size();
00263 
00264   cloud_ = (float*)malloc (original_no_of_points * dim_ * sizeof (float));
00265   float* cloud_ptr = cloud_;
00266   index_mapping_.reserve(original_no_of_points);
00267   identity_mapping_ = true;
00268 
00269   for (int indices_index = 0; indices_index < original_no_of_points; ++indices_index)
00270   {
00271     int cloud_index = indices[indices_index];
00272     const PointT point = cloud.points[cloud_index];
00273     // Check if the point is invalid
00274     if (!point_representation_->isValid(point)) {
00275       identity_mapping_ = false;
00276       continue;
00277     }
00278 
00279     index_mapping_.push_back(indices_index);  // If the returned index should be for the indices vector
00280     //index_mapping_.push_back(cloud_index);  // If the returned index should be for the ros cloud
00281     
00282     point_representation_->vectorize(point, cloud_ptr);
00283     cloud_ptr += dim_;
00284   }
00285 }
00286 
00287 #define PCL_INSTANTIATE_KdTreeFLANN(T) template class PCL_EXPORTS pcl::KdTreeFLANN<T>;
00288 
00289 #endif  //#ifndef _PCL_KDTREE_KDTREE_IMPL_FLANN_H_
00290 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines