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_KDTREE_KDTREE_IMPL_FLANN_H_
00039 #define PCL_KDTREE_KDTREE_IMPL_FLANN_H_
00040
00041 #include <pcl/kdtree/kdtree_flann.h>
00042 #include <pcl/console/print.h>
00043
00045 template <typename PointT, typename Dist> void
00046 pcl::KdTreeFLANN<PointT, Dist>::setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices)
00047 {
00048 cleanup ();
00049
00050 epsilon_ = 0.0f;
00051 dim_ = point_representation_->getNumberOfDimensions ();
00052
00053 input_ = cloud;
00054 indices_ = indices;
00055
00056
00057 if (!input_)
00058 {
00059 PCL_ERROR ("[pcl::KdTreeFLANN::setInputCloud] Invalid input!\n");
00060 return;
00061 }
00062 if (indices != NULL)
00063 {
00064 convertCloudToArray (*input_, *indices_);
00065 }
00066 else
00067 {
00068 convertCloudToArray (*input_);
00069 }
00070 total_nr_points_ = static_cast<int> (index_mapping_.size ());
00071
00072 flann_index_ = new FLANNIndex (flann::Matrix<float> (cloud_, index_mapping_.size (), dim_),
00073 flann::KDTreeSingleIndexParams (15));
00074 flann_index_->buildIndex ();
00075 }
00076
00078 template <typename PointT, typename Dist> int
00079 pcl::KdTreeFLANN<PointT, Dist>::nearestKSearch (const PointT &point, int k,
00080 std::vector<int> &k_indices,
00081 std::vector<float> &k_distances) const
00082 {
00083 assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
00084
00085 if (k > total_nr_points_)
00086 k = total_nr_points_;
00087
00088 k_indices.resize (k);
00089 k_distances.resize (k);
00090
00091 std::vector<float> query (dim_);
00092 point_representation_->vectorize (static_cast<PointT> (point), query);
00093
00094 flann::Matrix<int> k_indices_mat (&k_indices[0], 1, k);
00095 flann::Matrix<float> k_distances_mat (&k_distances[0], 1, k);
00096
00097 flann_index_->knnSearch (flann::Matrix<float> (&query[0], 1, dim_),
00098 k_indices_mat, k_distances_mat,
00099 k, param_k_);
00100
00101
00102 if (!identity_mapping_)
00103 {
00104 for (size_t i = 0; i < static_cast<size_t> (k); ++i)
00105 {
00106 int& neighbor_index = k_indices[i];
00107 neighbor_index = index_mapping_[neighbor_index];
00108 }
00109 }
00110
00111 return (k);
00112 }
00113
00115 template <typename PointT, typename Dist> int
00116 pcl::KdTreeFLANN<PointT, Dist>::radiusSearch (const PointT &point, double radius, std::vector<int> &k_indices,
00117 std::vector<float> &k_sqr_dists, unsigned int max_nn) const
00118 {
00119 assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!");
00120
00121 std::vector<float> query (dim_);
00122 point_representation_->vectorize (static_cast<PointT> (point), query);
00123
00124
00125 if (max_nn == 0 || max_nn > static_cast<unsigned int> (total_nr_points_))
00126 max_nn = total_nr_points_;
00127
00128 std::vector<std::vector<int> > indices(1);
00129 std::vector<std::vector<float> > dists(1);
00130
00131 flann::SearchParams params(param_radius_);
00132 if (max_nn == static_cast<unsigned int>(total_nr_points_))
00133 params.max_neighbors = -1;
00134 else
00135 params.max_neighbors = max_nn;
00136
00137 int neighbors_in_radius = flann_index_->radiusSearch (flann::Matrix<float> (&query[0], 1, dim_),
00138 indices,
00139 dists,
00140 static_cast<float> (radius * radius),
00141 params);
00142
00143 k_indices = indices[0];
00144 k_sqr_dists = dists[0];
00145
00146
00147 if (!identity_mapping_)
00148 {
00149 for (int i = 0; i < neighbors_in_radius; ++i)
00150 {
00151 int& neighbor_index = k_indices[i];
00152 neighbor_index = index_mapping_[neighbor_index];
00153 }
00154 }
00155
00156 return (neighbors_in_radius);
00157 }
00158
00160 template <typename PointT, typename Dist> void
00161 pcl::KdTreeFLANN<PointT, Dist>::cleanup ()
00162 {
00163 if (flann_index_)
00164 delete flann_index_;
00165
00166
00167 if (cloud_)
00168 {
00169 free (cloud_);
00170 cloud_ = NULL;
00171 }
00172 index_mapping_.clear ();
00173
00174 if (indices_)
00175 indices_.reset ();
00176 }
00177
00179 template <typename PointT, typename Dist> void
00180 pcl::KdTreeFLANN<PointT, Dist>::convertCloudToArray (const PointCloud &cloud)
00181 {
00182
00183 if (cloud.points.empty ())
00184 {
00185 cloud_ = NULL;
00186 return;
00187 }
00188
00189 int original_no_of_points = static_cast<int> (cloud.points.size ());
00190
00191 cloud_ = static_cast<float*> (malloc (original_no_of_points * dim_ * sizeof (float)));
00192 float* cloud_ptr = cloud_;
00193 index_mapping_.reserve (original_no_of_points);
00194 identity_mapping_ = true;
00195
00196 for (int cloud_index = 0; cloud_index < original_no_of_points; ++cloud_index)
00197 {
00198
00199 if (!point_representation_->isValid (cloud.points[cloud_index]))
00200 {
00201 identity_mapping_ = false;
00202 continue;
00203 }
00204
00205 index_mapping_.push_back (cloud_index);
00206
00207 point_representation_->vectorize (cloud.points[cloud_index], cloud_ptr);
00208 cloud_ptr += dim_;
00209 }
00210 }
00211
00213 template <typename PointT, typename Dist> void
00214 pcl::KdTreeFLANN<PointT, Dist>::convertCloudToArray (const PointCloud &cloud, const std::vector<int> &indices)
00215 {
00216
00217 if (cloud.points.empty ())
00218 {
00219 cloud_ = NULL;
00220 return;
00221 }
00222
00223 int original_no_of_points = static_cast<int> (indices.size ());
00224
00225 cloud_ = static_cast<float*> (malloc (original_no_of_points * dim_ * sizeof (float)));
00226 float* cloud_ptr = cloud_;
00227 index_mapping_.reserve (original_no_of_points);
00228
00229
00230
00231
00232
00233
00234
00235 identity_mapping_ = false;
00236
00237 for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
00238 {
00239
00240 if (!point_representation_->isValid (cloud.points[*iIt]))
00241 continue;
00242
00243
00244 index_mapping_.push_back (*iIt);
00245
00246 point_representation_->vectorize (cloud.points[*iIt], cloud_ptr);
00247 cloud_ptr += dim_;
00248 }
00249 }
00250
00251 #define PCL_INSTANTIATE_KdTreeFLANN(T) template class PCL_EXPORTS pcl::KdTreeFLANN<T>;
00252
00253 #endif //#ifndef _PCL_KDTREE_KDTREE_IMPL_FLANN_H_
00254