00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
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
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
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
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
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
00160
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
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)
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)
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)
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)
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_