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 in any of their floating point fields)...
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