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>
49 template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
52 salient_radius_ = salient_radius;
56 template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
59 non_max_radius_ = non_max_radius;
63 template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
66 normal_radius_ = normal_radius;
70 template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
73 border_radius_ = border_radius;
77 template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
84 template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
91 template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
94 min_neighbors_ = min_neighbors;
98 template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
105 template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
bool*
108 bool* edge_points =
new bool [input.size ()];
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++)
123 edge_points[index] =
false;
124 PointInT current_point = input[index];
129 std::vector<float> nn_distances;
132 this->searchForNeighbors (index, border_radius, nn_indices, nn_distances);
134 n_neighbors =
static_cast<int> (nn_indices.size ());
136 if (n_neighbors >= min_neighbors_)
140 if (boundary_estimator.
isBoundaryPoint (input,
static_cast<int> (index), nn_indices, u, v, angle_threshold))
141 edge_points[index] =
true;
146 return (edge_points);
150 template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
153 const PointInT& current_point = (*input_)[current_index];
155 double central_point[3];
156 memset(central_point, 0,
sizeof(
double) * 3);
158 central_point[0] = current_point.x;
159 central_point[1] = current_point.y;
160 central_point[2] = current_point.z;
162 cov_m = Eigen::Matrix3d::Zero ();
165 std::vector<float> nn_distances;
168 this->searchForNeighbors (current_index, salient_radius_, nn_indices, nn_distances);
170 n_neighbors =
static_cast<int> (nn_indices.size ());
172 if (n_neighbors < min_neighbors_)
176 memset(cov, 0,
sizeof(
double) * 9);
178 for (
const auto& n_idx : nn_indices)
180 const PointInT& n_point = (*input_)[n_idx];
182 double neigh_point[3];
183 memset(neigh_point, 0,
sizeof(
double) * 3);
185 neigh_point[0] = n_point.x;
186 neigh_point[1] = n_point.y;
187 neigh_point[2] = n_point.z;
189 for (
int i = 0; i < 3; i++)
190 for (
int j = 0; j < 3; j++)
191 cov[i * 3 + j] += (neigh_point[i] - central_point[i]) * (neigh_point[j] - central_point[j]);
194 cov_m << cov[0], cov[1], cov[2],
195 cov[3], cov[4], cov[5],
196 cov[6], cov[7], cov[8];
200 template<
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 )
263 normal_estimation.
compute (*normal_ptr);
271 normal_estimation.
compute (*normal_ptr);
273 normals_ = normal_ptr;
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_);
292 template<
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++)
309 borders[index] =
false;
310 PointInT current_point = (*input_)[index];
312 if ((border_radius_ > 0.0) && (
pcl::isFinite(current_point)))
315 std::vector<float> nn_distances;
317 this->searchForNeighbors (index, border_radius_, nn_indices, nn_distances);
319 for (
const auto &nn_index : nn_indices)
321 if (edge_points_[nn_index])
323 borders[index] =
true;
331 Eigen::Vector3d *omp_mem =
new Eigen::Vector3d[threads_];
333 for (std::size_t i = 0; i < threads_; i++)
334 omp_mem[i].setZero (3);
336 Eigen::Vector3d *omp_mem =
new Eigen::Vector3d[1];
338 omp_mem[0].setZero (3);
341 double *prg_local_mem =
new double[input_->size () * 3];
342 double **prg_mem =
new double * [input_->size ()];
344 for (std::size_t i = 0; i < input_->size (); i++)
345 prg_mem[i] = prg_local_mem + 3 * i;
347 #pragma omp parallel for \
349 shared(borders, omp_mem, prg_mem) \
350 num_threads(threads_)
351 for (
int index = 0; index < static_cast<int> (input_->size ()); index++)
354 int tid = omp_get_thread_num ();
358 PointInT current_point = (*input_)[index];
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);
382 omp_mem[tid][0] = e2c / e1c;
383 omp_mem[tid][1] = e3c / e2c;;
384 omp_mem[tid][2] = e3c;
387 for (Eigen::Index d = 0; d < omp_mem[tid].size (); d++)
388 prg_mem[index][d] = omp_mem[tid][d];
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++)
408 feat_max [index] =
false;
409 PointInT current_point = (*input_)[index];
411 if ((third_eigen_value_[index] > 0.0) && (
pcl::isFinite(current_point)))
414 std::vector<float> nn_distances;
417 this->searchForNeighbors (index, non_max_radius_, nn_indices, nn_distances);
419 n_neighbors =
static_cast<int> (nn_indices.size ());
421 if (n_neighbors >= min_neighbors_)
425 for (
const auto& j : nn_indices)
426 if (third_eigen_value_[index] < third_eigen_value_[j])
429 feat_max[index] =
true;
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;
451 output.width = output.size ();
455 if (border_radius_ > 0.0)
460 delete[] prg_local_mem;
465 #define PCL_INSTANTIATE_ISSKeypoint3D(T,U,N) template class PCL_EXPORTS pcl::ISSKeypoint3D<T,U,N>;
BoundaryEstimation estimates whether a set of points is lying on surface boundaries using an angle cr...
void getCoordinateSystemOnPlane(const PointNT &p_coeff, Eigen::Vector4f &u, Eigen::Vector4f &v)
Get a u-v-n coordinate system that lies on a plane defined by its normal.
bool isBoundaryPoint(const pcl::PointCloud< PointInT > &cloud, int q_idx, const pcl::Indices &indices, const Eigen::Vector4f &u, const Eigen::Vector4f &v, const float angle_threshold)
Check whether a point is a boundary point in a planar patch of projected points given by indices.
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
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.
void setNormalEstimationMethod(NormalEstimationMethod normal_estimation_method)
Set the normal estimation method.
void setInputCloud(const typename PointCloudIn::ConstPtr &cloud) override
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
Keypoint represents the base class for key points.
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point.
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
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.