38 #ifndef PCL_SIFT_KEYPOINT_IMPL_H_
39 #define PCL_SIFT_KEYPOINT_IMPL_H_
41 #include <pcl/keypoints/sift_keypoint.h>
42 #include <pcl/common/io.h>
43 #include <pcl/filters/voxel_grid.h>
46 template <
typename Po
intInT,
typename Po
intOutT>
void
49 min_scale_ = min_scale;
50 nr_octaves_ = nr_octaves;
51 nr_scales_per_octave_ = nr_scales_per_octave;
56 template <
typename Po
intInT,
typename Po
intOutT>
void
59 min_contrast_ = min_contrast;
63 template <
typename Po
intInT,
typename Po
intOutT>
bool
68 PCL_ERROR (
"[pcl::%s::initCompute] : Minimum scale (%f) must be strict positive!\n",
69 name_.c_str (), min_scale_);
74 PCL_ERROR (
"[pcl::%s::initCompute] : Number of octaves (%d) must be at least 1!\n",
75 name_.c_str (), nr_octaves_);
78 if (nr_scales_per_octave_ < 1)
80 PCL_ERROR (
"[pcl::%s::initCompute] : Number of scales per octave (%d) must be at least 1!\n",
81 name_.c_str (), nr_scales_per_octave_);
84 if (min_contrast_ < 0)
86 PCL_ERROR (
"[pcl::%s::initCompute] : Minimum contrast (%f) must be non-negative!\n",
87 name_.c_str (), min_contrast_);
97 template <
typename Po
intInT,
typename Po
intOutT>
void
100 if (surface_ && surface_ != input_)
102 PCL_WARN (
"[pcl::%s::detectKeypoints] : ", name_.c_str ());
103 PCL_WARN (
"A search surface has been set by setSearchSurface, but this SIFT keypoint detection algorithm does ");
104 PCL_WARN (
"not support search surfaces other than the input cloud. ");
105 PCL_WARN (
"The cloud provided in setInputCloud is being used instead.\n");
109 scale_idx_ = pcl::getFieldIndex<PointOutT> (output,
"scale", out_fields_);
112 output.points.clear ();
119 float scale = min_scale_;
120 for (
int i_octave = 0; i_octave < nr_octaves_; ++i_octave)
123 const float s = 1.0f * scale;
127 voxel_grid.
filter (*temp);
131 const size_t min_nr_points = 25;
132 if (cloud->points.size () < min_nr_points)
136 tree_->setInputCloud (cloud);
139 detectKeypointsForOctave (*cloud, *tree_, scale, nr_scales_per_octave_, output);
146 output.width =
static_cast<uint32_t
> (output.points.size ());
151 template <
typename Po
intInT,
typename Po
intOutT>
void
153 const PointCloudIn &input,
KdTree &tree,
float base_scale,
int nr_scales_per_octave,
154 PointCloudOut &output)
157 std::vector<float> scales (nr_scales_per_octave + 3);
158 for (
int i_scale = 0; i_scale <= nr_scales_per_octave + 2; ++i_scale)
160 scales[i_scale] = base_scale * powf (2.0f, (1.0f * static_cast<float> (i_scale) - 1.0f) / static_cast<float> (nr_scales_per_octave));
162 Eigen::MatrixXf diff_of_gauss;
163 computeScaleSpace (input, tree, scales, diff_of_gauss);
166 std::vector<int> extrema_indices, extrema_scales;
167 findScaleSpaceExtrema (input, tree, diff_of_gauss, extrema_indices, extrema_scales);
169 output.points.reserve (output.points.size () + extrema_indices.size ());
171 if (scale_idx_ != -1)
174 for (
size_t i_keypoint = 0; i_keypoint < extrema_indices.size (); ++i_keypoint)
177 const int &keypoint_index = extrema_indices[i_keypoint];
179 keypoint.x = input.points[keypoint_index].x;
180 keypoint.y = input.points[keypoint_index].y;
181 keypoint.z = input.points[keypoint_index].z;
182 memcpy (reinterpret_cast<char*> (&keypoint) + out_fields_[scale_idx_].offset,
183 &scales[extrema_scales[i_keypoint]],
sizeof (
float));
184 output.points.push_back (keypoint);
190 for (
size_t i_keypoint = 0; i_keypoint < extrema_indices.size (); ++i_keypoint)
193 const int &keypoint_index = extrema_indices[i_keypoint];
195 keypoint.x = input.points[keypoint_index].x;
196 keypoint.y = input.points[keypoint_index].y;
197 keypoint.z = input.points[keypoint_index].z;
199 output.points.push_back (keypoint);
206 template <
typename Po
intInT,
typename Po
intOutT>
208 const PointCloudIn &input, KdTree &tree,
const std::vector<float> &scales,
209 Eigen::MatrixXf &diff_of_gauss)
211 diff_of_gauss.resize (input.size (), scales.size () - 1);
214 const float max_radius = 3.0f * scales.back ();
216 for (
int i_point = 0; i_point < static_cast<int> (input.size ()); ++i_point)
218 std::vector<int> nn_indices;
219 std::vector<float> nn_dist;
220 tree.radiusSearch (i_point, max_radius, nn_indices, nn_dist);
226 float filter_response = 0.0f;
227 float previous_filter_response;
228 for (
size_t i_scale = 0; i_scale < scales.size (); ++i_scale)
230 float sigma_sqr = powf (scales[i_scale], 2.0f);
232 float numerator = 0.0f;
233 float denominator = 0.0f;
234 for (
size_t i_neighbor = 0; i_neighbor < nn_indices.size (); ++i_neighbor)
236 const float &value = getFieldValue_ (input.points[nn_indices[i_neighbor]]);
237 const float &dist_sqr = nn_dist[i_neighbor];
238 if (dist_sqr <= 9*sigma_sqr)
240 float w = expf (-0.5f * dist_sqr / sigma_sqr);
241 numerator += value * w;
246 previous_filter_response = filter_response;
247 filter_response = numerator / denominator;
251 diff_of_gauss (i_point, i_scale - 1) = filter_response - previous_filter_response;
257 template <
typename Po
intInT,
typename Po
intOutT>
void
259 const PointCloudIn &input, KdTree &tree,
const Eigen::MatrixXf &diff_of_gauss,
260 std::vector<int> &extrema_indices, std::vector<int> &extrema_scales)
263 std::vector<int> nn_indices (k);
264 std::vector<float> nn_dist (k);
266 const int nr_scales =
static_cast<int> (diff_of_gauss.cols ());
267 std::vector<float> min_val (nr_scales), max_val (nr_scales);
269 for (
int i_point = 0; i_point < static_cast<int> (input.size ()); ++i_point)
272 const size_t nr_nn = tree.nearestKSearch (i_point, k, nn_indices, nn_dist);
278 for (
int i_scale = 0; i_scale < nr_scales; ++i_scale)
280 min_val[i_scale] = std::numeric_limits<float>::max ();
281 max_val[i_scale] = -std::numeric_limits<float>::max ();
283 for (
size_t i_neighbor = 0; i_neighbor < nr_nn; ++i_neighbor)
285 const float &d = diff_of_gauss (nn_indices[i_neighbor], i_scale);
287 min_val[i_scale] = (std::min) (min_val[i_scale], d);
288 max_val[i_scale] = (std::max) (max_val[i_scale], d);
293 for (
int i_scale = 1; i_scale < nr_scales - 1; ++i_scale)
295 const float &val = diff_of_gauss (i_point, i_scale);
298 if (fabs (val) >= min_contrast_)
301 if ((val == min_val[i_scale]) &&
302 (val < min_val[i_scale - 1]) &&
303 (val < min_val[i_scale + 1]))
305 extrema_indices.push_back (i_point);
306 extrema_scales.push_back (i_scale);
309 else if ((val == max_val[i_scale]) &&
310 (val > max_val[i_scale - 1]) &&
311 (val > max_val[i_scale + 1]))
313 extrema_indices.push_back (i_point);
314 extrema_scales.push_back (i_scale);
321 #define PCL_INSTANTIATE_SIFTKeypoint(T,U) template class PCL_EXPORTS pcl::SIFTKeypoint<T,U>;
323 #endif // #ifndef PCL_SIFT_KEYPOINT_IMPL_H_
SIFTKeypoint detects the Scale Invariant Feature Transform keypoints for a given point cloud dataset ...
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
void filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data...
void setScales(float min_scale, int nr_octaves, int nr_scales_per_octave)
Specify the range of scales over which to search for keypoints.
void setMinimumContrast(float min_contrast)
Provide a threshold to limit detection of keypoints without sufficient contrast.
void detectKeypoints(PointCloudOut &output)
Detect the SIFT keypoints for a set of points given in setInputCloud () using the spatial locator in ...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
KdTree represents the base spatial locator class for kd-tree implementations.
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.