40 #ifndef PCL_HARRIS_KEYPOINT_2D_IMPL_H_ 41 #define PCL_HARRIS_KEYPOINT_2D_IMPL_H_ 44 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void 51 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void 54 threshold_= threshold;
58 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void 65 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void 72 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void 75 window_width_= window_width;
79 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void 82 window_height_= window_height;
86 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void 89 skipped_pixels_= skipped_pixels;
93 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void 96 min_distance_ = min_distance;
100 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void 103 static const int width = static_cast<int> (input_->width);
104 static const int height = static_cast<int> (input_->height);
106 int x = static_cast<int> (index % input_->width);
107 int y = static_cast<int> (index / input_->width);
110 memset (coefficients, 0,
sizeof (
float) * 3);
112 int endx = std::min (width, x + half_window_width_);
113 int endy = std::min (height, y + half_window_height_);
114 for (
int xx = std::max (0, x - half_window_width_); xx < endx; ++xx)
115 for (
int yy = std::max (0, y - half_window_height_); yy < endy; ++yy)
117 const float& ix = derivatives_rows_ (xx,yy);
118 const float& iy = derivatives_cols_ (xx,yy);
119 coefficients[0]+= ix * ix;
120 coefficients[1]+= ix * iy;
121 coefficients[2]+= iy * iy;
126 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
bool 131 PCL_ERROR (
"[pcl::%s::initCompute] init failed.!\n", name_.c_str ());
135 if (!input_->isOrganized ())
137 PCL_ERROR (
"[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ());
141 if (indices_->size () != input_->size ())
143 PCL_ERROR (
"[pcl::%s::initCompute] %s doesn't support setting indices!\n", name_.c_str ());
147 if ((window_height_%2) == 0)
149 PCL_ERROR (
"[pcl::%s::initCompute] Window height must be odd!\n", name_.c_str ());
153 if ((window_width_%2) == 0)
155 PCL_ERROR (
"[pcl::%s::initCompute] Window width must be odd!\n", name_.c_str ());
159 if (window_height_ < 3 || window_width_ < 3)
161 PCL_ERROR (
"[pcl::%s::initCompute] Window size must be >= 3x3!\n", name_.c_str ());
165 half_window_width_ = window_width_ / 2;
166 half_window_height_ = window_height_ / 2;
172 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void 175 derivatives_cols_.resize (input_->width, input_->height);
176 derivatives_rows_.resize (input_->width, input_->height);
179 int w = static_cast<int> (input_->width) - 1;
180 int h = static_cast<int> (input_->height) - 1;
183 derivatives_cols_(0,0) = (intensity_ ((*input_) (0,1)) - intensity_ ((*input_) (0,0))) * 0.5;
184 derivatives_rows_(0,0) = (intensity_ ((*input_) (1,0)) - intensity_ ((*input_) (0,0))) * 0.5;
186 for(
int i = 1; i < w; ++i)
188 derivatives_cols_(i,0) = (intensity_ ((*input_) (i,1)) - intensity_ ((*input_) (i,0))) * 0.5;
191 derivatives_rows_(w,0) = (intensity_ ((*input_) (w,0)) - intensity_ ((*input_) (w-1,0))) * 0.5;
192 derivatives_cols_(w,0) = (intensity_ ((*input_) (w,1)) - intensity_ ((*input_) (w,0))) * 0.5;
194 for(
int j = 1; j < h; ++j)
197 derivatives_rows_(0,j) = (intensity_ ((*input_) (1,j)) - intensity_ ((*input_) (0,j))) * 0.5;
198 for(
int i = 1; i < w; ++i)
201 derivatives_rows_(i,j) = (intensity_ ((*input_) (i+1,j)) - intensity_ ((*input_) (i-1,j))) * 0.5;
204 derivatives_cols_(i,j) = (intensity_ ((*input_) (i,j+1)) - intensity_ ((*input_) (i,j-1))) * 0.5;
207 derivatives_rows_(w,j) = (intensity_ ((*input_) (w,j)) - intensity_ ((*input_) (w-1,j))) * 0.5;
211 derivatives_cols_(0,h) = (intensity_ ((*input_) (0,h)) - intensity_ ((*input_) (0,h-1))) * 0.5;
212 derivatives_rows_(0,h) = (intensity_ ((*input_) (1,h)) - intensity_ ((*input_) (0,h))) * 0.5;
214 for(
int i = 1; i < w; ++i)
216 derivatives_cols_(i,h) = (intensity_ ((*input_) (i,h)) - intensity_ ((*input_) (i,h-1))) * 0.5;
218 derivatives_rows_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w-1,h))) * 0.5;
219 derivatives_cols_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w,h-1))) * 0.5;
224 responseHarris(*response_);
227 responseNoble(*response_);
230 responseLowe(*response_);
233 responseTomasi(*response_);
240 for (
size_t i = 0; i < response_->size (); ++i)
241 keypoints_indices_->indices.
push_back (i);
245 std::sort (indices_->begin (), indices_->end (),
246 boost::bind (&HarrisKeypoint2D::greaterIntensityAtIndices,
this, _1, _2));
247 float threshold = threshold_ * response_->points[indices_->front ()].intensity;
249 output.
reserve (response_->size());
250 std::vector<bool> occupency_map (response_->size (),
false);
251 int width (response_->width);
252 int height (response_->height);
253 const int occupency_map_size (occupency_map.size ());
256 #pragma omp parallel for shared (output, occupency_map) firstprivate (width, height) num_threads(threads_) 258 for (
int i = 0; i < occupency_map_size; ++i)
260 int idx = indices_->at (i);
261 const PointOutT& point_out = response_->points [idx];
262 if (occupency_map[idx] || point_out.intensity < threshold || !
isFinite (point_out))
270 keypoints_indices_->indices.push_back (idx);
273 int u_end = std::min (width, idx % width + min_distance_);
274 int v_end = std::min (height, idx / width + min_distance_);
275 for(
int u = std::max (0, idx % width - min_distance_); u < u_end; ++u)
276 for(
int v = std::max (0, idx / width - min_distance_); v < v_end; ++v)
277 occupency_map[v*input_->width+u] =
true;
284 output.
width = static_cast<uint32_t> (output.
size());
292 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void 295 PCL_ALIGN (16)
float covar [3];
297 output.
resize (input_->size ());
298 const int output_size (output.
size ());
301 #pragma omp parallel for shared (output) private (covar) num_threads(threads_) 303 for (
int index = 0; index < output_size; ++index)
305 PointOutT& out_point = output.
points [index];
306 const PointInT &in_point = (*input_).points [index];
307 out_point.intensity = 0;
308 out_point.x = in_point.x;
309 out_point.y = in_point.y;
310 out_point.z = in_point.z;
313 computeSecondMomentMatrix (index, covar);
314 float trace = covar [0] + covar [2];
317 float det = covar[0] * covar[2] - covar[1] * covar[1];
318 out_point.intensity = 0.04f + det - 0.04f * trace * trace;
323 output.
height = input_->height;
324 output.
width = input_->width;
328 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void 331 PCL_ALIGN (16)
float covar [3];
333 output.
resize (input_->size ());
334 const int output_size (output.
size ());
337 #pragma omp parallel for shared (output) private (covar) num_threads(threads_) 339 for (
int index = 0; index < output_size; ++index)
341 PointOutT &out_point = output.
points [index];
342 const PointInT &in_point = input_->points [index];
343 out_point.x = in_point.x;
344 out_point.y = in_point.y;
345 out_point.z = in_point.z;
346 out_point.intensity = 0;
349 computeSecondMomentMatrix (index, covar);
350 float trace = covar [0] + covar [2];
353 float det = covar[0] * covar[2] - covar[1] * covar[1];
354 out_point.intensity = det / trace;
359 output.
height = input_->height;
360 output.
width = input_->width;
364 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void 367 PCL_ALIGN (16)
float covar [3];
369 output.
resize (input_->size ());
370 const int output_size (output.
size ());
373 #pragma omp parallel for shared (output) private (covar) num_threads(threads_) 375 for (
int index = 0; index < output_size; ++index)
377 PointOutT &out_point = output.
points [index];
378 const PointInT &in_point = input_->points [index];
379 out_point.x = in_point.x;
380 out_point.y = in_point.y;
381 out_point.z = in_point.z;
382 out_point.intensity = 0;
385 computeSecondMomentMatrix (index, covar);
386 float trace = covar [0] + covar [2];
389 float det = covar[0] * covar[2] - covar[1] * covar[1];
390 out_point.intensity = det / (trace * trace);
395 output.
height = input_->height;
396 output.
width = input_->width;
400 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void 403 PCL_ALIGN (16)
float covar [3];
405 output.
resize (input_->size ());
406 const int output_size (output.
size ());
409 #pragma omp parallel for shared (output) private (covar) num_threads(threads_) 411 for (
int index = 0; index < output_size; ++index)
413 PointOutT &out_point = output.
points [index];
414 const PointInT &in_point = input_->points [index];
415 out_point.x = in_point.x;
416 out_point.y = in_point.y;
417 out_point.z = in_point.z;
418 out_point.intensity = 0;
421 computeSecondMomentMatrix (index, covar);
423 out_point.intensity = ((covar[0] + covar[2] - sqrt((covar[0] - covar[2])*(covar[0] - covar[2]) + 4 * covar[1] * covar[1])) /2.0f);
427 output.
height = input_->height;
428 output.
width = input_->width;
475 #define PCL_INSTANTIATE_HarrisKeypoint2D(T,U,I) template class PCL_EXPORTS pcl::HarrisKeypoint2D<T,U,I>; 476 #endif // #ifndef PCL_HARRIS_KEYPOINT_2D_IMPL_H_ bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
void setMethod(ResponseMethod type)
set the method of the response to be calculated.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void setWindowHeight(int window_height)
Set window height.
void setNonMaxSupression(bool=false)
whether non maxima suppression should be applied or the response for each point should be returned
void setMinimalDistance(int min_distance)
Set minimal distance between candidate keypoints.
void responseTomasi(PointCloudOut &output) const
void setRefine(bool do_refine)
whether the detected key points should be refined or not.
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
uint32_t height
The point cloud height (if organized as an image-structure).
Keypoint represents the base class for key points.
uint32_t width
The point cloud width (if organized as an image-structure).
void clear()
Removes all points in a cloud and sets the width and height to 0.
void responseHarris(PointCloudOut &output) const
gets the corner response for valid input points
void responseLowe(PointCloudOut &output) const
PointCloud represents the base class in PCL for storing collections of 3D points.
void detectKeypoints(PointCloudOut &output)
Abstract key point detection method.
void setSkippedPixels(int skipped_pixels)
Set number of pixels to skip.
void setWindowWidth(int window_width)
Set window width.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
void setThreshold(float threshold)
set the threshold value for detecting corners.
void resize(size_t n)
Resize the cloud.
void computeSecondMomentMatrix(std::size_t pos, float *coefficients) const
calculates the upper triangular part of unnormalized covariance matrix over intensities given by the ...
void responseNoble(PointCloudOut &output) const