Point Cloud Library (PCL)
1.7.0
|
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 the copyright holder(s) 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 00044 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00052 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00070 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00138 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00217 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00272 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00333 ////////////////////////////////////////////////////////////////////////////////////////////// 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_