38#ifndef PCL_ISS_KEYPOINT3D_IMPL_H_
39#define PCL_ISS_KEYPOINT3D_IMPL_H_
41#include <Eigen/Eigenvalues>
42#include <pcl/features/boundary.h>
43#include <pcl/features/normal_3d.h>
44#include <pcl/features/integral_image_normal.h>
46#include <pcl/keypoints/iss_3d.h>
49template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
56template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
63template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
70template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
77template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
84template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
91template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
98template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
105template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
bool*
110 Eigen::Vector4f u = Eigen::Vector4f::Zero ();
111 Eigen::Vector4f v = Eigen::Vector4f::Zero ();
116#pragma omp parallel for \
118 shared(angle_threshold, boundary_estimator, border_radius, edge_points, input) \
120 num_threads(threads_)
121 for (
int index = 0; index <
int (
input.
size ()); index++)
150template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
162 cov_m = Eigen::Matrix3d::Zero ();
176 memset(cov, 0,
sizeof(
double) * 9);
189 for (
int i = 0; i < 3; i++)
190 for (
int j = 0; j < 3; j++)
194 cov_m << cov[0], cov[1], cov[2],
195 cov[3], cov[4], cov[5],
196 cov[6], cov[7], cov[8];
200template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
bool
205 PCL_ERROR (
"[pcl::%s::initCompute] init failed!\n", name_.c_str ());
208 if (salient_radius_ <= 0)
210 PCL_ERROR (
"[pcl::%s::initCompute] : the salient radius (%f) must be strict positive!\n",
211 name_.c_str (), salient_radius_);
214 if (non_max_radius_ <= 0)
216 PCL_ERROR (
"[pcl::%s::initCompute] : the non maxima radius (%f) must be strict positive!\n",
217 name_.c_str (), non_max_radius_);
222 PCL_ERROR (
"[pcl::%s::initCompute] : the threshold on the ratio between the 2nd and the 1rst eigenvalue (%f) must be strict positive!\n",
223 name_.c_str (), gamma_21_);
228 PCL_ERROR (
"[pcl::%s::initCompute] : the threshold on the ratio between the 3rd and the 2nd eigenvalue (%f) must be strict positive!\n",
229 name_.c_str (), gamma_32_);
232 if (min_neighbors_ <= 0)
234 PCL_ERROR (
"[pcl::%s::initCompute] : the minimum number of neighbors (%f) must be strict positive!\n",
235 name_.c_str (), min_neighbors_);
239 delete[] third_eigen_value_;
241 third_eigen_value_ =
new double[input_->size ()];
242 memset(third_eigen_value_, 0,
sizeof(
double) * input_->size ());
244 delete[] edge_points_;
246 if (border_radius_ > 0.0)
248 if (normals_->empty ())
250 if (normal_radius_ <= 0.)
252 PCL_ERROR (
"[pcl::%s::initCompute] : the radius used to estimate surface normals (%f) must be positive!\n",
253 name_.c_str (), normal_radius_);
258 if (input_->height == 1 )
275 if (normals_->size () != surface_->size ())
277 PCL_ERROR (
"[pcl::%s::initCompute] normals given, but the number of normals does not match the number of input points!\n", name_.c_str ());
281 else if (border_radius_ < 0.0)
283 PCL_ERROR (
"[pcl::%s::initCompute] : the border radius used to estimate boundary points (%f) must be positive!\n",
284 name_.c_str (), border_radius_);
292template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
298 if (border_radius_ > 0.0)
299 edge_points_ = getBoundaryPoints (*(input_->makeShared ()), border_radius_, angle_threshold_);
301 bool*
borders =
new bool [input_->size()];
303#pragma omp parallel for \
306 num_threads(threads_)
307 for (
int index = 0; index <
int (input_->size ()); index++)
331 Eigen::Vector3d *
omp_mem =
new Eigen::Vector3d[threads_];
333 for (std::size_t i = 0; i < threads_; i++)
336 Eigen::Vector3d *
omp_mem =
new Eigen::Vector3d[1];
342 double **
prg_mem =
new double * [input_->size ()];
344 for (std::size_t i = 0; i < input_->size (); i++)
347#pragma omp parallel for \
349 shared(borders, omp_mem, prg_mem) \
350 num_threads(threads_)
363 Eigen::Matrix3d
cov_m = Eigen::Matrix3d::Zero ();
364 getScatterMatrix (
static_cast<int> (index),
cov_m);
366 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d>
solver (
cov_m);
368 const double&
e1c =
solver.eigenvalues ()[2];
369 const double&
e2c =
solver.eigenvalues ()[1];
370 const double&
e3c =
solver.eigenvalues ()[0];
372 if (!std::isfinite (
e1c) || !std::isfinite (
e2c) || !std::isfinite (
e3c))
377 PCL_WARN (
"[pcl::%s::detectKeypoints] : The third eigenvalue is negative! Skipping the point with index %i.\n",
378 name_.c_str (), index);
391 for (
int index = 0; index <
int (input_->size ()); index++)
395 if ((
prg_mem[index][0] < gamma_21_) && (
prg_mem[index][1] < gamma_32_))
396 third_eigen_value_[index] =
prg_mem[index][2];
400 bool*
feat_max =
new bool [input_->size()];
402#pragma omp parallel for \
405 num_threads(threads_)
406 for (
int index = 0; index <
int (input_->size ()); index++)
426 if (third_eigen_value_[index] < third_eigen_value_[j])
434#pragma omp parallel for \
436 shared(feat_max, output) \
437 num_threads(threads_)
438 for (
int index = 0; index <
int (input_->size ()); index++)
444 p.getVector3fMap () = (*input_)[index].getVector3fMap ();
446 keypoints_indices_->indices.push_back (index);
450 output.header = input_->header;
455 if (border_radius_ > 0.0)
465#define PCL_INSTANTIATE_ISSKeypoint3D(T,U,N) template class PCL_EXPORTS pcl::ISSKeypoint3D<T,U,N>;
Iterator class for point clouds with or without given indices.
ConstCloudIterator(const PointCloud< PointT > &cloud)
std::size_t size() const
Size of the range the iterator is going through.
typename PointCloudN::ConstPtr PointCloudNConstPtr
typename Keypoint< PointInT, PointOutT >::PointCloudIn PointCloudIn
void setNormals(const PointCloudNConstPtr &normals)
Set the normals if pre-calculated normals are available.
void setBorderRadius(double border_radius)
Set the radius used for the estimation of the boundary points.
void setSalientRadius(double salient_radius)
Set the radius of the spherical neighborhood used to compute the scatter matrix.
void getScatterMatrix(const int ¤t_index, Eigen::Matrix3d &cov_m)
Compute the scatter matrix for a point index.
void setThreshold21(double gamma_21)
Set the upper bound on the ratio between the second and the first eigenvalue.
void setMinNeighbors(int min_neighbors)
Set the minimum number of neighbors that has to be found while applying the non maxima suppression al...
void setNormalRadius(double normal_radius)
Set the radius used for the estimation of the surface normals of the input cloud.
void setThreshold32(double gamma_32)
Set the upper bound on the ratio between the third and the second eigenvalue.
bool initCompute() override
Perform the initial checks before computing the keypoints.
typename PointCloudN::Ptr PointCloudNPtr
void setNonMaxRadius(double non_max_radius)
Set the radius for the application of the non maxima supression algorithm.
bool * getBoundaryPoints(PointCloudIn &input, double border_radius, float angle_threshold)
Compute the boundary points for the given input cloud.
void detectKeypoints(PointCloudOut &output) override
Detect the keypoints by performing the EVD of the scatter matrix.
typename Keypoint< PointInT, PointOutT >::PointCloudOut PointCloudOut
Surface normal estimation on organized data using integral images.
Keypoint represents the base class for key points.
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...