Point Cloud Library (PCL)
1.3.1
|
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