Point Cloud Library (PCL)  1.3.1
organized.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: organized.hpp 3212 2011-11-18 17:27:32Z rusu $
00037  *
00038  */
00039 
00040 #ifndef PCL_SEARCH_IMPL_ORGANIZED_NEIGHBOR_SEARCH_H_
00041 #define PCL_SEARCH_IMPL_ORGANIZED_NEIGHBOR_SEARCH_H_
00042 
00043 #include "pcl/search/organized.h"
00044 
00046 template<typename PointT> int
00047 pcl::search::OrganizedNeighbor<PointT>::radiusSearch (const pcl::PointCloud<PointT> &cloud, int index,
00048                                                             double radius, std::vector<int> &k_indices,
00049                                                             std::vector<float> &k_distances, int max_nn)
00050 {
00051   k_indices.clear ();
00052   k_distances.clear ();
00053 
00054   if (!cloud.isOrganized ())
00055   {
00056     PCL_ERROR ("[pcl::%s::nearestKSearch] Input dataset is not organized!\n", getName ().c_str ());
00057     return (0);
00058   }
00059   int data_size = cloud.points.size ();
00060   if (index >= data_size)
00061     return (0);
00062 
00063   // Get the cloud's dimensions width
00064   int width = cloud.width, height = cloud.height;
00065 
00066   int y = index / width, x = index - y * width;
00067 
00068   if (!pcl_isfinite (cloud.points[index].x) ||
00069       !pcl_isfinite (cloud.points[index].y) ||
00070       !pcl_isfinite (cloud.points[index].z))
00071     return (0);
00072 
00073   // Put point itself into results
00074   k_indices.push_back (index);
00075   k_distances.push_back (0.0f);
00076 
00077   float max_dist_squared = radius * radius;
00078   bool still_in_range = true, done = false;
00079   for (int radius = 1; !done; ++radius)
00080   {
00081     int x2 = x - radius - 1, y2 = y - radius; // Top left - 1
00082     still_in_range = false;
00083     for (int i = 0; i < 8 * radius; ++i)
00084     {
00085       if (i <= 2 * radius)
00086         ++x2;
00087       else if (i <= 4 * radius)
00088         ++y2;
00089       else if (i <= 6 * radius)
00090         --x2;
00091       else
00092         --y2;
00093       if (x2 < 0 || x2 >= width || y2 < 0 || y2 >= height)
00094         continue;
00095       int neighbor_index = y2 * width + x2;
00096 
00097       if (!pcl_isfinite (cloud.points[neighbor_index].x) ||
00098           !pcl_isfinite (cloud.points[neighbor_index].y) ||
00099           !pcl_isfinite (cloud.points[neighbor_index].z))
00100         continue;
00101 
00102       float distance_squared = squaredEuclideanDistance (cloud.points[index], cloud.points[neighbor_index]);
00103       if (distance_squared > max_dist_squared)
00104         continue;
00105 
00106       still_in_range = true;
00107       // The capacity should change here if the new neighborhood has more points than the previous one
00108       k_indices.push_back (neighbor_index);
00109       k_distances.push_back (distance_squared);
00110       if ((int)k_indices.size () >= max_nn)
00111       {
00112         done = true;
00113         break;
00114       }
00115     }
00116     if (!still_in_range)
00117       done = true;
00118   }
00119 
00120   return (int (k_indices.size ()));
00121 }
00122 
00124 template<typename PointT> int
00125 pcl::search::OrganizedNeighbor<PointT>::radiusSearch (int index,
00126                                                             double radius, std::vector<int> &k_indices,
00127                                                             std::vector<float> &k_distances, int max_nn) const
00128 {
00129   k_indices.clear ();
00130   k_distances.clear ();
00131 
00132   if (!input_)
00133   {
00134     PCL_ERROR ("[pcl::%s::radiusSearch] Input dataset was not set! use setInputCloud before continuing.\n", getName ().c_str ());
00135     return (0);
00136   }
00137   if (!input_->isOrganized ())
00138   {
00139     PCL_ERROR ("[pcl::%s::radiusSearch] Input dataset is not organized!\n", getName ().c_str ());
00140     return 0;
00141   }
00142   int data_size = input_->points.size ();
00143   if (index >= data_size)
00144     return (0);
00145 
00146   // Get the cloud's dimensions width
00147   int width = input_->width, height = input_->height;
00148 
00149   int y = index / width, x = index - y * width;
00150 
00151   if (!pcl_isfinite (input_->points[index].x) ||
00152       !pcl_isfinite (input_->points[index].y) ||
00153       !pcl_isfinite (input_->points[index].z))
00154     return (0);
00155 
00156   // Put point itself into results. The vector capacity should be unchanged.
00157   k_indices.push_back (index);
00158   k_distances.push_back (0.0f);
00159 
00160   float max_dist_squared = radius * radius;
00161   bool still_in_range = true, done = false;
00162   for (int radius = 1; !done; ++radius)
00163   {
00164     int x2 = x - radius - 1, y2 = y - radius; // Top left - 1
00165     still_in_range = false;
00166     for (int i = 0; i < 8 * radius; ++i)
00167     {
00168       if (i <= 2 * radius)
00169         ++x2;
00170       else if (i <= 4 * radius)
00171         ++y2;
00172       else if (i <= 6 * radius)
00173         --x2;
00174       else
00175         --y2;
00176       if (x2 < 0 || x2 >= width || y2 < 0 || y2 >= height)
00177         continue;
00178       int neighbor_index = y2 * width + x2;
00179 
00180       if (!pcl_isfinite (input_->points[neighbor_index].x) ||
00181           !pcl_isfinite (input_->points[neighbor_index].y) ||
00182           !pcl_isfinite (input_->points[neighbor_index].z))
00183         continue;
00184 
00185       float distance_squared = squaredEuclideanDistance (input_->points[index], input_->points[neighbor_index]);
00186       if (distance_squared > max_dist_squared)
00187         continue;
00188 
00189       still_in_range = true;
00190       // The capacity should change here if the new neighborhood has more points than the previous one
00191       k_indices.push_back (neighbor_index);
00192       k_distances.push_back (distance_squared);
00193       if ((int)k_indices.size () >= max_nn)
00194       {
00195         done = true;
00196         break;
00197       }
00198     }
00199     if (!still_in_range)
00200       done = true;
00201   }
00202 
00203   return (int (k_indices.size ()));
00204 }
00205 
00209 template<typename PointT> int
00210 pcl::search::OrganizedNeighbor<PointT>::nearestKSearch (int index, int k,
00211                                                               std::vector<int> &k_indices,
00212                                                               std::vector<float> &k_distances)
00213 {
00214   if (!input_)
00215   {
00216     PCL_ERROR ("[pcl::%s::approxNearestKSearch] Input dataset was not set! use setInputCloud before continuing.\n", getName ().c_str ());
00217     return (0);
00218   }
00219 
00220   k_indices.resize (k);
00221   if (!input_->isOrganized ())
00222   {
00223     PCL_ERROR ("[pcl::%s::approxNearestKSearch] Input dataset is not organized!\n", getName ().c_str ());
00224     return (0);
00225   }
00226   int data_size = input_->points.size ();
00227   if (index >= data_size)
00228     return (0);
00229 
00230   // Get the cloud width
00231   int width = input_->width;
00232 
00233   // Obtain the <u,v> pixel values
00234   int u = index / width;
00235   int v = index % width;
00236 
00237   int l = -1, idx, uwv = u * width + v, uwvx;
00238 
00239   // Save the query point as the first neighbor (*ANN compatibility)
00240   k_indices[++l] = index;
00241 
00242   if (horizontal_window_ == 0 || vertical_window_)
00243     setSearchWindowAsK (k);
00244 
00245   // Get all point neighbors in a H x V window
00246 
00247   for (int x = -horizontal_window_; x != horizontal_window_; ++x)
00248   {
00249     uwvx = uwv + x * width; // Get the correct index
00250 
00251     for (int y = -vertical_window_; y != vertical_window_; ++y)
00252     {
00253       // idx = (u+x) * cloud.width + (v+y);
00254       idx = uwvx + y;
00255 
00256       // If the index is not in the point cloud, continue
00257       if (idx == index || idx < 0 || idx >= data_size)
00258         continue;
00259 
00260       if (max_distance_ != 0)
00261       {
00262         if (fabs (input_->points[index].z - input_->points[index].z) < max_distance_)
00263           k_indices[++l] = idx;
00264       }
00265       else
00266         k_indices[++l] = idx;
00267     }
00268   }
00269   // We need at least min_pts_ nearest neighbors to do something useful with them
00270   if (l < min_pts_)
00271     return (0);
00272   return (k);
00273 }
00274 
00276 template<typename PointT> int
00277 pcl::search::OrganizedNeighbor<PointT>::nearestKSearch (const pcl::PointCloud<PointT> &cloud, int index, 
00278                                                               int k,
00279                                                               std::vector<int> &k_indices,
00280                                                               std::vector<float> &k_distances)
00281 {
00282   k_indices.resize (k);
00283   if (!cloud.isOrganized ())
00284   {
00285     PCL_ERROR ("[pcl::%s::approxNearestKSearch] Input dataset is not organized!\n", getName ().c_str ());
00286     return (0);
00287   }
00288   int data_size = cloud.points.size ();
00289   if (index >= data_size)
00290     return (0);
00291 
00292   // Get the cloud width
00293   int width = cloud.width;
00294 
00295   // Obtain the <u,v> pixel values
00296   int u = index / width;
00297   int v = index % width;
00298 
00299   int l = -1, idx, uwv = u * width + v, uwvx;
00300 
00301   // Save the query point as the first neighbor (*ANN compatibility)
00302   k_indices[++l] = index;
00303 
00304   if (horizontal_window_ == 0 || vertical_window_)
00305     setSearchWindowAsK (k);
00306 
00307   // Get all point neighbors in a H x V window
00308 
00309   for (int x = -horizontal_window_; x != horizontal_window_; ++x)
00310   {
00311     uwvx = uwv + x * width; // Get the correct index
00312 
00313     for (int y = -vertical_window_; y != vertical_window_; ++y)
00314     {
00315       // idx = (u+x) * cloud.width + (v+y);
00316       idx = uwvx + y;
00317 
00318       // If the index is not in the point cloud, continue
00319       if (idx == index || idx < 0 || idx >= data_size)
00320         continue;
00321 
00322       if (max_distance_ != 0)
00323       {
00324         if (fabs (cloud.points[index].z - cloud.points[idx].z) < max_distance_)
00325           k_indices[++l] = idx;
00326       }
00327       else
00328         k_indices[++l] = idx;
00329     }
00330   }
00331   // We need at least min_pts_ nearest neighbors to do something useful with them
00332   if (l < min_pts_)
00333     return (0);
00334   return (k);
00335 }
00336 
00338 template<typename PointT> void
00339 pcl::search::OrganizedNeighbor<PointT>::setSearchWindowAsK (int k)
00340 {
00341   int hw = 0, vw = 0;
00342   while ((2 * hw + 1) * (2 * vw + 1) < k)
00343   {
00344     ++hw;
00345     ++vw;
00346   }
00347   horizontal_window_ = hw - 1;
00348   vertical_window_ = vw - 1;
00349 }
00350 
00351 #define PCL_INSTANTIATE_OrganizedNeighbor(T) template class PCL_EXPORTS pcl::search::OrganizedNeighbor<T>;
00352 
00353 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines