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);
111 memset (coefficients, 0,
sizeof (
float) * 3);
113 int endx = std::min (width, x + half_window_width_);
114 int endy = std::min (height, y + half_window_height_);
115 for (
int xx = std::max (0, x - half_window_width_); xx < endx; ++xx)
116 for (
int yy = std::max (0, y - half_window_height_); yy < endy; ++yy)
118 const float& ix = derivatives_rows_ (xx,yy);
119 const float& iy = derivatives_cols_ (xx,yy);
120 coefficients[0]+= ix * ix;
121 coefficients[1]+= ix * iy;
122 coefficients[2]+= iy * iy;
127 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
bool
132 PCL_ERROR (
"[pcl::%s::initCompute] init failed.!\n", name_.c_str ());
136 if (!input_->isOrganized ())
138 PCL_ERROR (
"[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ());
142 if (indices_->size () != input_->size ())
144 PCL_ERROR (
"[pcl::%s::initCompute] %s doesn't support setting indices!\n", name_.c_str ());
148 if ((window_height_%2) == 0)
150 PCL_ERROR (
"[pcl::%s::initCompute] Window height must be odd!\n", name_.c_str ());
154 if ((window_width_%2) == 0)
156 PCL_ERROR (
"[pcl::%s::initCompute] Window width must be odd!\n", name_.c_str ());
160 if (window_height_ < 3 || window_width_ < 3)
162 PCL_ERROR (
"[pcl::%s::initCompute] Window size must be >= 3x3!\n", name_.c_str ());
166 half_window_width_ = window_width_ / 2;
167 half_window_height_ = window_height_ / 2;
173 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
176 derivatives_cols_.resize (input_->width, input_->height);
177 derivatives_rows_.resize (input_->width, input_->height);
180 int w =
static_cast<int> (input_->width) - 1;
181 int h =
static_cast<int> (input_->height) - 1;
184 derivatives_cols_(0,0) = (intensity_ ((*input_) (0,1)) - intensity_ ((*input_) (0,0))) * 0.5;
185 derivatives_rows_(0,0) = (intensity_ ((*input_) (1,0)) - intensity_ ((*input_) (0,0))) * 0.5;
191 for(
int i = 1; i < w; ++i)
193 derivatives_cols_(i,0) = (intensity_ ((*input_) (i,1)) - intensity_ ((*input_) (i,0))) * 0.5;
196 derivatives_rows_(w,0) = (intensity_ ((*input_) (w,0)) - intensity_ ((*input_) (w-1,0))) * 0.5;
197 derivatives_cols_(w,0) = (intensity_ ((*input_) (w,1)) - intensity_ ((*input_) (w,0))) * 0.5;
203 for(
int j = 1; j < h; ++j)
206 derivatives_rows_(0,j) = (intensity_ ((*input_) (1,j)) - intensity_ ((*input_) (0,j))) * 0.5;
207 for(
int i = 1; i < w; ++i)
210 derivatives_rows_(i,j) = (intensity_ ((*input_) (i+1,j)) - intensity_ ((*input_) (i-1,j))) * 0.5;
213 derivatives_cols_(i,j) = (intensity_ ((*input_) (i,j+1)) - intensity_ ((*input_) (i,j-1))) * 0.5;
216 derivatives_rows_(w,j) = (intensity_ ((*input_) (w,j)) - intensity_ ((*input_) (w-1,j))) * 0.5;
220 derivatives_cols_(0,h) = (intensity_ ((*input_) (0,h)) - intensity_ ((*input_) (0,h-1))) * 0.5;
221 derivatives_rows_(0,h) = (intensity_ ((*input_) (1,h)) - intensity_ ((*input_) (0,h))) * 0.5;
227 for(
int i = 1; i < w; ++i)
229 derivatives_cols_(i,h) = (intensity_ ((*input_) (i,h)) - intensity_ ((*input_) (i,h-1))) * 0.5;
231 derivatives_rows_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w-1,h))) * 0.5;
232 derivatives_cols_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w,h-1))) * 0.5;
234 float highest_response_;
239 responseHarris(*response_, highest_response_);
242 responseNoble(*response_, highest_response_);
245 responseLowe(*response_, highest_response_);
248 responseTomasi(*response_, highest_response_);
256 threshold_*= highest_response_;
258 std::sort (indices_->begin (), indices_->end (),
259 boost::bind (&HarrisKeypoint2D::greaterIntensityAtIndices,
this, _1, _2));
262 output.
reserve (response_->size());
263 std::vector<bool> occupency_map (response_->size (),
false);
264 int width (response_->width);
265 int height (response_->height);
266 const int occupency_map_size (occupency_map.size ());
269 #pragma omp parallel for shared (output, occupency_map) private (width, height) num_threads(threads_)
271 for (
int idx = 0; idx < occupency_map_size; ++idx)
273 if (occupency_map[idx] || response_->points [indices_->at (idx)].intensity < threshold_ || !
isFinite (response_->points[idx]))
279 output.
push_back (response_->at (indices_->at (idx)));
281 int u_end = std::min (width, indices_->at (idx) % width + min_distance_);
282 int v_end = std::min (height, indices_->at (idx) / width + min_distance_);
283 for(
int u = std::max (0, indices_->at (idx) % width - min_distance_); u < u_end; ++u)
284 for(
int v = std::max (0, indices_->at (idx) / width - min_distance_); v < v_end; ++v)
285 occupency_map[v*input_->width+u] =
true;
292 output.
width =
static_cast<uint32_t
> (output.
size());
300 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
303 PCL_ALIGN (16)
float covar [3];
305 output.
resize (input_->size ());
306 highest_response = - std::numeric_limits<float>::max ();
307 const int output_size (output.
size ());
310 #pragma omp parallel for shared (output, highest_response) private (covar) num_threads(threads_)
312 for (
int index = 0; index < output_size; ++index)
314 PointOutT& out_point = output.
points [index];
315 const PointInT &in_point = (*input_).points [index];
316 out_point.intensity = 0;
317 out_point.x = in_point.x;
318 out_point.y = in_point.y;
319 out_point.z = in_point.z;
322 computeSecondMomentMatrix (index, covar);
323 float trace = covar [0] + covar [2];
326 float det = covar[0] * covar[2] - covar[1] * covar[1];
327 out_point.intensity = 0.04f + det - 0.04f * trace * trace;
332 highest_response = (out_point.intensity > highest_response) ? out_point.intensity : highest_response;
337 output.
height = input_->height;
338 output.
width = input_->width;
342 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
345 PCL_ALIGN (16)
float covar [3];
347 output.
resize (input_->size ());
348 highest_response = - std::numeric_limits<float>::max ();
349 const int output_size (output.
size ());
352 #pragma omp parallel for shared (output, highest_response) private (covar) num_threads(threads_)
354 for (
size_t index = 0; index < output_size; ++index)
356 PointOutT &out_point = output.
points [index];
357 const PointInT &in_point = input_->points [index];
358 out_point.x = in_point.x;
359 out_point.y = in_point.y;
360 out_point.z = in_point.z;
361 out_point.intensity = 0;
364 computeSecondMomentMatrix (index, covar);
365 float trace = covar [0] + covar [2];
368 float det = covar[0] * covar[2] - covar[1] * covar[1];
369 out_point.intensity = det / trace;
374 highest_response = (out_point.intensity > highest_response) ? out_point.intensity : highest_response;
379 output.
height = input_->height;
380 output.
width = input_->width;
384 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
387 PCL_ALIGN (16)
float covar [3];
389 output.
resize (input_->size ());
390 highest_response = -std::numeric_limits<float>::max ();
391 const int output_size (output.
size ());
394 #pragma omp parallel for shared (output, highest_response) private (covar) num_threads(threads_)
396 for (
size_t index = 0; index < output_size; ++index)
398 PointOutT &out_point = output.
points [index];
399 const PointInT &in_point = input_->points [index];
400 out_point.x = in_point.x;
401 out_point.y = in_point.y;
402 out_point.z = in_point.z;
403 out_point.intensity = 0;
406 computeSecondMomentMatrix (index, covar);
407 float trace = covar [0] + covar [2];
410 float det = covar[0] * covar[2] - covar[1] * covar[1];
411 out_point.intensity = det / (trace * trace);
416 highest_response = (out_point.intensity > highest_response) ? out_point.intensity : highest_response;
421 output.
height = input_->height;
422 output.
width = input_->width;
426 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
429 PCL_ALIGN (16)
float covar [3];
431 output.
resize (input_->size ());
432 highest_response = -std::numeric_limits<float>::max ();
433 const int output_size (output.
size ());
436 #pragma omp parallel for shared (output, highest_response) private (covar) num_threads(threads_)
438 for (
size_t index = 0; index < output_size; ++index)
440 PointOutT &out_point = output.
points [index];
441 const PointInT &in_point = input_->points [index];
442 out_point.x = in_point.x;
443 out_point.y = in_point.y;
444 out_point.z = in_point.z;
445 out_point.intensity = 0;
448 computeSecondMomentMatrix (index, covar);
450 out_point.intensity = ((covar[0] + covar[2] - sqrt((covar[0] - covar[2])*(covar[0] - covar[2]) + 4 * covar[1] * covar[1])) /2.0f);
455 highest_response = (out_point.intensity > highest_response) ? out_point.intensity : highest_response;
459 output.
height = input_->height;
460 output.
width = input_->width;
507 #define PCL_INSTANTIATE_HarrisKeypoint2D(T,U,I) template class PCL_EXPORTS pcl::HarrisKeypoint2D<T,U,I>;
508 #endif // #ifndef PCL_HARRIS_KEYPOINT_2D_IMPL_H_
void responseHarris(PointCloudOut &output, float &highest_response) const
gets the corner response for valid input points
void setMethod(ResponseMethod type)
set the method of the response to be calculated.
void setThreshold(float threshold)
set the threshold value for detecting corners.
void setRefine(bool do_refine)
whether the detected key points should be refined or not.
uint32_t height
The point cloud height (if organized as an image-structure).
void responseNoble(PointCloudOut &output, float &highest_response) const
void responseTomasi(PointCloudOut &output, float &highest_response) const
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested.
void setWindowHeight(int window_height)
Set window height.
void computeSecondMomentMatrix(std::size_t pos, float *coefficients) const
calculates the upper triangular part of unnormalized covariance matrix over intensities given by the ...
void resize(size_t n)
Resize the cloud.
Keypoint represents the base class for key points.
void setSkippedPixels(int skipped_pixels)
Set number of pixels to skip.
uint32_t width
The point cloud width (if organized as an image-structure).
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void responseLowe(PointCloudOut &output, float &highest_response) const
void setWindowWidth(int window_width)
Set window width.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void detectKeypoints(PointCloudOut &output)
Abstract key point detection method.
void clear()
Removes all points in a cloud and sets the width and height to 0.
void setNonMaxSupression(bool=false)
whether non maxima suppression should be applied or the response for each point should be returned ...
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
void setMinimalDistance(int min_distance)
Set minimal distance between candidate keypoints.