38#ifndef PCL_HARRIS_KEYPOINT_3D_IMPL_H_
39#define PCL_HARRIS_KEYPOINT_3D_IMPL_H_
41#include <pcl/keypoints/harris_3d.h>
42#include <pcl/common/io.h>
43#include <pcl/features/normal_3d.h>
44#include <pcl/features/integral_image_normal.h>
51template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
54 if (normals_ && input_ && (cloud != input_))
60template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
67template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
70 threshold_= threshold;
74template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
77 search_radius_ = radius;
81template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
88template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
95template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
102template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
120 for (
const auto &neighbor : neighbors)
122 if (std::isfinite ((*normals_)[neighbor].normal_x))
145 zz += (*normals_)[neighbor].normal_z * (*normals_)[neighbor].normal_z;
156 coefficients [7] =
zz /
float(count);
159 memset (coefficients, 0,
sizeof (
float) * 8);
161 memset (coefficients, 0,
sizeof (
float) * 8);
162 for (
const auto& index : neighbors)
164 if (std::isfinite ((*normals_)[index].normal_x))
166 coefficients[0] += (*normals_)[index].normal_x * (*normals_)[index].normal_x;
167 coefficients[1] += (*normals_)[index].normal_x * (*normals_)[index].normal_y;
168 coefficients[2] += (*normals_)[index].normal_x * (*normals_)[index].normal_z;
170 coefficients[5] += (*normals_)[index].normal_y * (*normals_)[index].normal_y;
171 coefficients[6] += (*normals_)[index].normal_y * (*normals_)[index].normal_z;
172 coefficients[7] += (*normals_)[index].normal_z * (*normals_)[index].normal_z;
179 float norm = 1.0 /
float (count);
180 coefficients[0] *= norm;
181 coefficients[1] *= norm;
182 coefficients[2] *= norm;
183 coefficients[5] *= norm;
184 coefficients[6] *= norm;
185 coefficients[7] *= norm;
191template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
bool
196 PCL_ERROR (
"[pcl::%s::initCompute] init failed!\n", name_.c_str ());
202 PCL_ERROR (
"[pcl::%s::initCompute] method (%d) must be in [1..5]!\n", name_.c_str (), method_);
209 normals->reserve (normals->size ());
210 if (!surface_->isOrganized ())
227 if (normals_->size () != surface_->size ())
229 PCL_ERROR (
"[pcl::%s::initCompute] normals given, but the number of normals does not match the number of input points!\n", name_.c_str (), method_);
237template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
242 response->points.reserve (input_->size());
247 responseHarris(*response);
250 responseNoble(*response);
253 responseLowe(*response);
256 responseCurvature(*response);
259 responseTomasi(*response);
267 output.is_dense = input_->is_dense;
268 for (std::size_t i = 0; i < response->size (); ++i)
269 keypoints_indices_->indices.push_back (i);
274 output.reserve (response->size());
276#pragma omp parallel for \
278 shared(output, response) \
279 num_threads(threads_)
283 !std::isfinite ((*response)[idx].intensity) ||
284 (*response)[idx].intensity < threshold_)
293 if ((*response)[idx].intensity < (*response)[index].intensity)
302 output.push_back ((*response)[idx]);
303 keypoints_indices_->indices.push_back (idx);
317template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
321 output.resize (input_->size ());
322#pragma omp parallel for \
325 firstprivate(covar) \
326 num_threads(threads_)
353 output.height = input_->height;
354 output.width = input_->width;
358template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
362 output.resize (input_->size ());
363#pragma omp parallel \
366 firstprivate(covar) \
367 num_threads(threads_)
393 output.height = input_->height;
394 output.width = input_->width;
398template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
402 output.resize (input_->size ());
403#pragma omp parallel for \
406 firstprivate(covar) \
407 num_threads(threads_)
433 output.height = input_->height;
434 output.width = input_->width;
438template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
442 for (std::size_t idx = 0; idx < input_->size(); ++idx)
444 point.x = (*input_)[idx].x;
445 point.y = (*input_)[idx].y;
446 point.z = (*input_)[idx].z;
447 point.intensity = (*normals_)[idx].curvature;
451 output.height = input_->height;
452 output.width = input_->width;
456template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
461 output.resize (input_->size ());
462#pragma omp parallel for \
465 firstprivate(covar, covariance_matrix) \
466 num_threads(threads_)
496 output.height = input_->height;
497 output.width = input_->width;
501template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
507 Eigen::Vector3f
NNTp;
509#pragma omp parallel for \
512 firstprivate(nnT, NNT, NNTInv, NNTp) \
513 num_threads(threads_)
529 if (!std::isfinite ((*normals_)[index].normal_x))
532 nnT = (*normals_)[index].getNormalVector3fMap () * (*normals_)[index].getNormalVector3fMap ().transpose();
534 NNTp +=
nnT * (*surface_)[index].getVector3fMap ();
547#define PCL_INSTANTIATE_HarrisKeypoint3D(T,U,N) template class PCL_EXPORTS pcl::HarrisKeypoint3D<T,U,N>;
Define methods for centroid estimation and covariance matrix calculus.
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
typename PointCloudN::Ptr PointCloudNPtr
void responseTomasi(PointCloudOut &output) const
void detectKeypoints(PointCloudOut &output) override
typename PointCloudN::ConstPtr PointCloudNConstPtr
void responseNoble(PointCloudOut &output) const
bool initCompute() override
void responseHarris(PointCloudOut &output) const
gets the corner response for valid input points
typename Keypoint< PointInT, PointOutT >::PointCloudOut PointCloudOut
void setNormals(const PointCloudNConstPtr &normals)
Set normals if precalculated normals are available.
void responseCurvature(PointCloudOut &output) const
void refineCorners(PointCloudOut &corners) const
void setRadius(float radius)
Set the radius for normal estimation and non maxima supression.
void setNonMaxSupression(bool=false)
Whether non maxima suppression should be applied or the response for each point should be returned.
void setInputCloud(const PointCloudInConstPtr &cloud) override
Provide a pointer to the input dataset.
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 responseLowe(PointCloudOut &output) const
void calculateNormalCovar(const pcl::Indices &neighbors, float *coefficients) const
calculates the upper triangular part of unnormalized covariance matrix over the normals given by the ...
typename PointCloudIn::ConstPtr PointCloudInConstPtr
void setRefine(bool do_refine)
Whether the detected key points should be refined or not.
Surface normal estimation on organized data using integral images.
Keypoint represents the base class for key points.
shared_ptr< PointCloud< PointT > > Ptr
Matrix::Scalar invert3x3SymMatrix(const Matrix &matrix, Matrix &inverse)
Calculate the inverse of a 3x3 symmetric matrix.
void eigen33(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi d...
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...
IndicesAllocator<> Indices
Type used for indices in PCL.