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);
147 output.width =
static_cast<uint32_t
> (output.points.size ());
148 output.header = input_->header;
149 output.sensor_origin_ = input_->sensor_origin_;
150 output.sensor_orientation_ = input_->sensor_orientation_;
155 template <
typename Po
intInT,
typename Po
intOutT>
void
157 const PointCloudIn &input,
KdTree &tree,
float base_scale,
int nr_scales_per_octave,
158 PointCloudOut &output)
161 std::vector<float> scales (nr_scales_per_octave + 3);
162 for (
int i_scale = 0; i_scale <= nr_scales_per_octave + 2; ++i_scale)
164 scales[i_scale] = base_scale * powf (2.0f, (1.0f * static_cast<float> (i_scale) - 1.0f) / static_cast<float> (nr_scales_per_octave));
166 Eigen::MatrixXf diff_of_gauss;
167 computeScaleSpace (input, tree, scales, diff_of_gauss);
170 std::vector<int> extrema_indices, extrema_scales;
171 findScaleSpaceExtrema (input, tree, diff_of_gauss, extrema_indices, extrema_scales);
173 output.points.reserve (output.points.size () + extrema_indices.size ());
175 if (scale_idx_ != -1)
178 for (
size_t i_keypoint = 0; i_keypoint < extrema_indices.size (); ++i_keypoint)
181 const int &keypoint_index = extrema_indices[i_keypoint];
183 keypoint.x = input.points[keypoint_index].x;
184 keypoint.y = input.points[keypoint_index].y;
185 keypoint.z = input.points[keypoint_index].z;
186 memcpy (reinterpret_cast<char*> (&keypoint) + out_fields_[scale_idx_].offset,
187 &scales[extrema_scales[i_keypoint]],
sizeof (
float));
188 output.points.push_back (keypoint);
194 for (
size_t i_keypoint = 0; i_keypoint < extrema_indices.size (); ++i_keypoint)
197 const int &keypoint_index = extrema_indices[i_keypoint];
199 keypoint.x = input.points[keypoint_index].x;
200 keypoint.y = input.points[keypoint_index].y;
201 keypoint.z = input.points[keypoint_index].z;
203 output.points.push_back (keypoint);
210 template <
typename Po
intInT,
typename Po
intOutT>
212 const PointCloudIn &input, KdTree &tree,
const std::vector<float> &scales,
213 Eigen::MatrixXf &diff_of_gauss)
215 diff_of_gauss.resize (input.size (), scales.size () - 1);
218 const float max_radius = 3.0f * scales.back ();
220 for (
int i_point = 0; i_point < static_cast<int> (input.size ()); ++i_point)
222 std::vector<int> nn_indices;
223 std::vector<float> nn_dist;
224 tree.radiusSearch (i_point, max_radius, nn_indices, nn_dist);
230 float filter_response = 0.0f;
231 float previous_filter_response;
232 for (
size_t i_scale = 0; i_scale < scales.size (); ++i_scale)
234 float sigma_sqr = powf (scales[i_scale], 2.0f);
236 float numerator = 0.0f;
237 float denominator = 0.0f;
238 for (
size_t i_neighbor = 0; i_neighbor < nn_indices.size (); ++i_neighbor)
240 const float &value = getFieldValue_ (input.points[nn_indices[i_neighbor]]);
241 const float &dist_sqr = nn_dist[i_neighbor];
242 if (dist_sqr <= 9*sigma_sqr)
244 float w = expf (-0.5f * dist_sqr / sigma_sqr);
245 numerator += value * w;
250 previous_filter_response = filter_response;
251 filter_response = numerator / denominator;
255 diff_of_gauss (i_point, i_scale - 1) = filter_response - previous_filter_response;
261 template <
typename Po
intInT,
typename Po
intOutT>
void
263 const PointCloudIn &input, KdTree &tree,
const Eigen::MatrixXf &diff_of_gauss,
264 std::vector<int> &extrema_indices, std::vector<int> &extrema_scales)
267 std::vector<int> nn_indices (k);
268 std::vector<float> nn_dist (k);
270 const int nr_scales =
static_cast<int> (diff_of_gauss.cols ());
271 std::vector<float> min_val (nr_scales), max_val (nr_scales);
273 for (
int i_point = 0; i_point < static_cast<int> (input.size ()); ++i_point)
276 const size_t nr_nn = tree.nearestKSearch (i_point, k, nn_indices, nn_dist);
282 for (
int i_scale = 0; i_scale < nr_scales; ++i_scale)
284 min_val[i_scale] = std::numeric_limits<float>::max ();
285 max_val[i_scale] = -std::numeric_limits<float>::max ();
287 for (
size_t i_neighbor = 0; i_neighbor < nr_nn; ++i_neighbor)
289 const float &d = diff_of_gauss (nn_indices[i_neighbor], i_scale);
291 min_val[i_scale] = (std::min) (min_val[i_scale], d);
292 max_val[i_scale] = (std::max) (max_val[i_scale], d);
297 for (
int i_scale = 1; i_scale < nr_scales - 1; ++i_scale)
299 const float &val = diff_of_gauss (i_point, i_scale);
302 if (fabs (val) >= min_contrast_)
305 if ((val == min_val[i_scale]) &&
306 (val < min_val[i_scale - 1]) &&
307 (val < min_val[i_scale + 1]))
309 extrema_indices.push_back (i_point);
310 extrema_scales.push_back (i_scale);
313 else if ((val == max_val[i_scale]) &&
314 (val > max_val[i_scale - 1]) &&
315 (val > max_val[i_scale + 1]))
317 extrema_indices.push_back (i_point);
318 extrema_scales.push_back (i_scale);
325 #define PCL_INSTANTIATE_SIFTKeypoint(T,U) template class PCL_EXPORTS pcl::SIFTKeypoint<T,U>;
327 #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.