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
00039
00040 #ifndef PCL_SEARCH_IMPL_FLANN_SEARCH_H_
00041 #define PCL_SEARCH_IMPL_FLANN_SEARCH_H_
00042
00043 #include <pcl/search/flann_search.h>
00044 #include <pcl/kdtree/flann.h>
00045
00047 template <typename PointT, typename FlannDistance>
00048 typename pcl::search::FlannSearch<PointT, FlannDistance>::IndexPtr
00049 pcl::search::FlannSearch<PointT, FlannDistance>::KdTreeIndexCreator::createIndex (MatrixConstPtr data)
00050 {
00051 return (IndexPtr (new flann::KDTreeSingleIndex<FlannDistance> (*data,flann::KDTreeSingleIndexParams (max_leaf_size_))));
00052 }
00053
00054
00056 template <typename PointT, typename FlannDistance>
00057 pcl::search::FlannSearch<PointT, FlannDistance>::FlannSearch(bool sorted, FlannIndexCreator *creator) : pcl::search::Search<PointT> ("FlannSearch",sorted),
00058 creator_ (creator), eps_ (0), input_copied_for_flann_ (false)
00059 {
00060 point_representation_.reset (new DefaultPointRepresentation<PointT>);
00061 dim_ = point_representation_->getNumberOfDimensions ();
00062 }
00063
00065 template <typename PointT, typename FlannDistance>
00066 pcl::search::FlannSearch<PointT, FlannDistance>::~FlannSearch()
00067 {
00068 delete creator_;
00069 if (input_copied_for_flann_)
00070 delete [] input_flann_->ptr();
00071 }
00072
00074 template <typename PointT, typename FlannDistance> void
00075 pcl::search::FlannSearch<PointT, FlannDistance>::setInputCloud (const PointCloudConstPtr& cloud, const IndicesConstPtr& indices)
00076 {
00077 input_ = cloud;
00078 indices_ = indices;
00079 convertInputToFlannMatrix ();
00080 index_ = creator_->createIndex (input_flann_);
00081 index_->buildIndex ();
00082 }
00083
00085 template <typename PointT, typename FlannDistance> int
00086 pcl::search::FlannSearch<PointT, FlannDistance>::nearestKSearch (const PointT &point, int k, std::vector<int> &indices, std::vector<float> &dists) const
00087 {
00088 assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
00089 bool can_cast = point_representation_->isTrivial ();
00090
00091 float* data = 0;
00092 if (!can_cast)
00093 {
00094 data = new float [point_representation_->getNumberOfDimensions ()];
00095 point_representation_->vectorize (point,data);
00096 }
00097
00098 float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&point)): data;
00099 const flann::Matrix<float> m (cdata ,1, point_representation_->getNumberOfDimensions ());
00100
00101 flann::SearchParams p(-1);
00102 p.eps = eps_;
00103 p.sorted = sorted_results_;
00104 if (indices.size() != static_cast<unsigned int> (k))
00105 indices.resize (k,-1);
00106 if (dists.size() != static_cast<unsigned int> (k))
00107 dists.resize (k);
00108 flann::Matrix<int> i (&indices[0],1,k);
00109 flann::Matrix<float> d (&dists[0],1,k);
00110 int result = index_->knnSearch (m,i,d,k, p);
00111
00112 delete [] data;
00113
00114 if (!identity_mapping_)
00115 {
00116 for (size_t i = 0; i < static_cast<unsigned int> (k); ++i)
00117 {
00118 int& neighbor_index = indices[i];
00119 neighbor_index = index_mapping_[neighbor_index];
00120 }
00121 }
00122 return result;
00123 }
00124
00126 template <typename PointT, typename FlannDistance> void
00127 pcl::search::FlannSearch<PointT, FlannDistance>::nearestKSearch (
00128 const PointCloud& cloud, const std::vector<int>& indices, int k, std::vector< std::vector<int> >& k_indices,
00129 std::vector< std::vector<float> >& k_sqr_distances) const
00130 {
00131 if (indices.empty ())
00132 {
00133 k_indices.resize (cloud.size ());
00134 k_sqr_distances.resize (cloud.size ());
00135
00136 if (! cloud.is_dense)
00137 {
00138 for (size_t i = 0; i < cloud.size(); i++)
00139 {
00140 assert (point_representation_->isValid (cloud[i]) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
00141 }
00142 }
00143
00144 bool can_cast = point_representation_->isTrivial ();
00145
00146
00147 float* data=0;
00148 if (!can_cast)
00149 {
00150 data = new float[dim_*cloud.size ()];
00151 for (size_t i = 0; i < cloud.size (); ++i)
00152 {
00153 float* out = data+i*dim_;
00154 point_representation_->vectorize (cloud[i],out);
00155 }
00156 }
00157
00158
00159
00160 float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&cloud[0])): data;
00161 const flann::Matrix<float> m (cdata ,cloud.size (), dim_, can_cast ? sizeof (PointT) : dim_ * sizeof (float) );
00162
00163 flann::SearchParams p;
00164 p.sorted = sorted_results_;
00165 p.eps = eps_;
00166 index_->knnSearch (m,k_indices,k_sqr_distances,k, p);
00167
00168 delete [] data;
00169 }
00170 else
00171 {
00172 k_indices.resize (indices.size ());
00173 k_sqr_distances.resize (indices.size ());
00174
00175 if (! cloud.is_dense)
00176 {
00177 for (size_t i = 0; i < indices.size(); i++)
00178 {
00179 assert (point_representation_->isValid (cloud [indices[i]]) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
00180 }
00181 }
00182
00183 float* data=new float [dim_*indices.size ()];
00184 for (size_t i = 0; i < indices.size (); ++i)
00185 {
00186 float* out = data+i*dim_;
00187 point_representation_->vectorize (cloud[indices[i]],out);
00188 }
00189 const flann::Matrix<float> m (data ,indices.size (), point_representation_->getNumberOfDimensions ());
00190
00191 flann::SearchParams p;
00192 p.sorted = sorted_results_;
00193 p.eps = eps_;
00194 index_->knnSearch (m,k_indices,k_sqr_distances,k, p);
00195
00196 delete[] data;
00197 }
00198 if (!identity_mapping_)
00199 {
00200 for (size_t j = 0; j < k_indices.size (); ++j)
00201 {
00202 for (size_t i = 0; i < static_cast<unsigned int> (k); ++i)
00203 {
00204 int& neighbor_index = k_indices[j][i];
00205 neighbor_index = index_mapping_[neighbor_index];
00206 }
00207 }
00208 }
00209 }
00210
00212 template <typename PointT, typename FlannDistance> int
00213 pcl::search::FlannSearch<PointT, FlannDistance>::radiusSearch (const PointT& point, double radius,
00214 std::vector<int> &indices, std::vector<float> &distances,
00215 unsigned int max_nn) const
00216 {
00217 assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!");
00218 bool can_cast = point_representation_->isTrivial ();
00219
00220 float* data = 0;
00221 if (!can_cast)
00222 {
00223 data = new float [point_representation_->getNumberOfDimensions ()];
00224 point_representation_->vectorize (point,data);
00225 }
00226
00227 float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&point)) : data;
00228 const flann::Matrix<float> m (cdata ,1, point_representation_->getNumberOfDimensions ());
00229
00230 flann::SearchParams p;
00231 p.sorted = sorted_results_;
00232 p.eps = eps_;
00233 p.max_neighbors = max_nn > 0 ? max_nn : -1;
00234 std::vector<std::vector<int> > i (1);
00235 std::vector<std::vector<float> > d (1);
00236 int result = index_->radiusSearch (m,i,d,static_cast<float> (radius * radius), p);
00237
00238 delete [] data;
00239 indices = i [0];
00240 distances = d [0];
00241
00242 if (!identity_mapping_)
00243 {
00244 for (size_t i = 0; i < indices.size (); ++i)
00245 {
00246 int& neighbor_index = indices [i];
00247 neighbor_index = index_mapping_ [neighbor_index];
00248 }
00249 }
00250 return result;
00251 }
00252
00254 template <typename PointT, typename FlannDistance> void
00255 pcl::search::FlannSearch<PointT, FlannDistance>::radiusSearch (
00256 const PointCloud& cloud, const std::vector<int>& indices, double radius, std::vector< std::vector<int> >& k_indices,
00257 std::vector< std::vector<float> >& k_sqr_distances, unsigned int max_nn) const
00258 {
00259 if (indices.empty ())
00260 {
00261 k_indices.resize (cloud.size ());
00262 k_sqr_distances.resize (cloud.size ());
00263
00264 if (! cloud.is_dense)
00265 {
00266 for (size_t i = 0; i < cloud.size(); i++)
00267 {
00268 assert (point_representation_->isValid (cloud[i]) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!");
00269 }
00270 }
00271
00272 bool can_cast = point_representation_->isTrivial ();
00273
00274 float* data = 0;
00275 if (!can_cast)
00276 {
00277 data = new float[dim_*cloud.size ()];
00278 for (size_t i = 0; i < cloud.size (); ++i)
00279 {
00280 float* out = data+i*dim_;
00281 point_representation_->vectorize (cloud[i],out);
00282 }
00283 }
00284
00285 float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&cloud[0])) : data;
00286 const flann::Matrix<float> m (cdata ,cloud.size (), dim_, can_cast ? sizeof (PointT) : dim_ * sizeof (float));
00287
00288 flann::SearchParams p;
00289 p.sorted = sorted_results_;
00290 p.eps = eps_;
00291
00292 p.max_neighbors = max_nn > 0 ? max_nn : -1;
00293 index_->radiusSearch (m,k_indices,k_sqr_distances,static_cast<float> (radius * radius), p);
00294
00295 delete [] data;
00296 }
00297 else
00298 {
00299 k_indices.resize (indices.size ());
00300 k_sqr_distances.resize (indices.size ());
00301
00302 if (! cloud.is_dense)
00303 {
00304 for (size_t i = 0; i < indices.size(); i++)
00305 {
00306 assert (point_representation_->isValid (cloud [indices[i]]) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!");
00307 }
00308 }
00309
00310 float* data = new float [dim_ * indices.size ()];
00311 for (size_t i = 0; i < indices.size (); ++i)
00312 {
00313 float* out = data+i*dim_;
00314 point_representation_->vectorize (cloud[indices[i]], out);
00315 }
00316 const flann::Matrix<float> m (data, cloud.size (), point_representation_->getNumberOfDimensions ());
00317
00318 flann::SearchParams p;
00319 p.sorted = sorted_results_;
00320 p.eps = eps_;
00321
00322 p.max_neighbors = max_nn > 0 ? max_nn : -1;
00323 index_->radiusSearch (m, k_indices, k_sqr_distances, static_cast<float> (radius * radius), p);
00324
00325 delete[] data;
00326 }
00327 if (!identity_mapping_)
00328 {
00329 for (size_t j = 0; j < k_indices.size (); ++j )
00330 {
00331 for (size_t i = 0; i < k_indices[j].size (); ++i)
00332 {
00333 int& neighbor_index = k_indices[j][i];
00334 neighbor_index = index_mapping_[neighbor_index];
00335 }
00336 }
00337 }
00338 }
00339
00341 template <typename PointT, typename FlannDistance> void
00342 pcl::search::FlannSearch<PointT, FlannDistance>::convertInputToFlannMatrix ()
00343 {
00344 size_t original_no_of_points = indices_ && !indices_->empty () ? indices_->size () : input_->size ();
00345
00346 if (input_copied_for_flann_)
00347 delete input_flann_->ptr();
00348 input_copied_for_flann_ = true;
00349 index_mapping_.clear();
00350 identity_mapping_ = true;
00351
00352
00353
00354
00355
00356 if (!indices_ || indices_->empty ())
00357 {
00358
00359 if (input_->is_dense && point_representation_->isTrivial ())
00360 {
00361
00362 input_flann_ = MatrixPtr (new flann::Matrix<float> (const_cast<float*>(reinterpret_cast<const float*>(&(*input_) [0])), original_no_of_points, point_representation_->getNumberOfDimensions (),sizeof (PointT)));
00363 input_copied_for_flann_ = false;
00364 }
00365 else
00366 {
00367 input_flann_ = MatrixPtr (new flann::Matrix<float> (new float[original_no_of_points*point_representation_->getNumberOfDimensions ()], original_no_of_points, point_representation_->getNumberOfDimensions ()));
00368 float* cloud_ptr = input_flann_->ptr();
00369 for (size_t i = 0; i < original_no_of_points; ++i)
00370 {
00371 const PointT& point = (*input_)[i];
00372
00373 if (!point_representation_->isValid (point))
00374 {
00375 identity_mapping_ = false;
00376 continue;
00377 }
00378
00379 index_mapping_.push_back (static_cast<int> (i));
00380
00381 point_representation_->vectorize (point, cloud_ptr);
00382 cloud_ptr += dim_;
00383 }
00384 }
00385
00386 }
00387 else
00388 {
00389 input_flann_ = MatrixPtr (new flann::Matrix<float> (new float[original_no_of_points*point_representation_->getNumberOfDimensions ()], original_no_of_points, point_representation_->getNumberOfDimensions ()));
00390 float* cloud_ptr = input_flann_->ptr();
00391 for (size_t indices_index = 0; indices_index < original_no_of_points; ++indices_index)
00392 {
00393 int cloud_index = (*indices_)[indices_index];
00394 const PointT& point = (*input_)[cloud_index];
00395
00396 if (!point_representation_->isValid (point))
00397 {
00398 identity_mapping_ = false;
00399 continue;
00400 }
00401
00402 index_mapping_.push_back (static_cast<int> (indices_index));
00403
00404 point_representation_->vectorize (point, cloud_ptr);
00405 cloud_ptr += dim_;
00406 }
00407 }
00408 if (input_copied_for_flann_)
00409 input_flann_->rows = index_mapping_.size ();
00410 }
00411
00412 #define PCL_INSTANTIATE_FlannSearch(T) template class PCL_EXPORTS pcl::search::FlannSearch<T>;
00413
00414 #endif