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>
void
110 threads_ = omp_get_num_procs();
115 threads_ = nr_threads;
119 template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
bool*
122 bool* edge_points =
new bool [input.size ()];
124 Eigen::Vector4f u = Eigen::Vector4f::Zero ();
125 Eigen::Vector4f v = Eigen::Vector4f::Zero ();
130 #pragma omp parallel for \
132 shared(angle_threshold, boundary_estimator, border_radius, edge_points, input) \
134 num_threads(threads_)
135 for (
int index = 0; index < int (input.size ()); index++)
137 edge_points[index] =
false;
138 PointInT current_point = input[index];
143 std::vector<float> nn_distances;
146 this->searchForNeighbors (index, border_radius, nn_indices, nn_distances);
148 n_neighbors =
static_cast<int> (nn_indices.size ());
150 if (n_neighbors >= min_neighbors_)
154 if (boundary_estimator.
isBoundaryPoint (input,
static_cast<int> (index), nn_indices, u, v, angle_threshold))
155 edge_points[index] =
true;
160 return (edge_points);
164 template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
167 const PointInT& current_point = (*input_)[current_index];
169 double central_point[3];
170 memset(central_point, 0,
sizeof(
double) * 3);
172 central_point[0] = current_point.x;
173 central_point[1] = current_point.y;
174 central_point[2] = current_point.z;
176 cov_m = Eigen::Matrix3d::Zero ();
179 std::vector<float> nn_distances;
182 this->searchForNeighbors (current_index, salient_radius_, nn_indices, nn_distances);
184 n_neighbors =
static_cast<int> (nn_indices.size ());
186 if (n_neighbors < min_neighbors_)
190 memset(cov, 0,
sizeof(
double) * 9);
192 for (
const auto& n_idx : nn_indices)
194 const PointInT& n_point = (*input_)[n_idx];
196 double neigh_point[3];
197 memset(neigh_point, 0,
sizeof(
double) * 3);
199 neigh_point[0] = n_point.x;
200 neigh_point[1] = n_point.y;
201 neigh_point[2] = n_point.z;
203 for (
int i = 0; i < 3; i++)
204 for (
int j = 0; j < 3; j++)
205 cov[i * 3 + j] += (neigh_point[i] - central_point[i]) * (neigh_point[j] - central_point[j]);
208 cov_m << cov[0], cov[1], cov[2],
209 cov[3], cov[4], cov[5],
210 cov[6], cov[7], cov[8];
214 template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
bool
219 PCL_ERROR (
"[pcl::%s::initCompute] init failed!\n", name_.c_str ());
222 if (salient_radius_ <= 0)
224 PCL_ERROR (
"[pcl::%s::initCompute] : the salient radius (%f) must be strict positive!\n",
225 name_.c_str (), salient_radius_);
228 if (non_max_radius_ <= 0)
230 PCL_ERROR (
"[pcl::%s::initCompute] : the non maxima radius (%f) must be strict positive!\n",
231 name_.c_str (), non_max_radius_);
236 PCL_ERROR (
"[pcl::%s::initCompute] : the threshold on the ratio between the 2nd and the 1rst eigenvalue (%f) must be strict positive!\n",
237 name_.c_str (), gamma_21_);
242 PCL_ERROR (
"[pcl::%s::initCompute] : the threshold on the ratio between the 3rd and the 2nd eigenvalue (%f) must be strict positive!\n",
243 name_.c_str (), gamma_32_);
246 if (min_neighbors_ <= 0)
248 PCL_ERROR (
"[pcl::%s::initCompute] : the minimum number of neighbors (%f) must be strict positive!\n",
249 name_.c_str (), min_neighbors_);
253 delete[] third_eigen_value_;
255 third_eigen_value_ =
new double[input_->size ()];
256 memset(third_eigen_value_, 0,
sizeof(
double) * input_->size ());
258 delete[] edge_points_;
260 if (border_radius_ > 0.0)
262 if (normals_->empty ())
264 if (normal_radius_ <= 0.)
266 PCL_ERROR (
"[pcl::%s::initCompute] : the radius used to estimate surface normals (%f) must be positive!\n",
267 name_.c_str (), normal_radius_);
272 if (input_->height == 1 )
277 normal_estimation.
compute (*normal_ptr);
285 normal_estimation.
compute (*normal_ptr);
287 normals_ = normal_ptr;
289 if (normals_->size () != surface_->size ())
291 PCL_ERROR (
"[pcl::%s::initCompute] normals given, but the number of normals does not match the number of input points!\n", name_.c_str ());
295 else if (border_radius_ < 0.0)
297 PCL_ERROR (
"[pcl::%s::initCompute] : the border radius used to estimate boundary points (%f) must be positive!\n",
298 name_.c_str (), border_radius_);
306 template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
312 if (border_radius_ > 0.0)
313 edge_points_ = getBoundaryPoints (*(input_->makeShared ()), border_radius_, angle_threshold_);
315 bool* borders =
new bool [input_->size()];
317 #pragma omp parallel for \
320 num_threads(threads_)
321 for (
int index = 0; index < int (input_->size ()); index++)
323 borders[index] =
false;
324 PointInT current_point = (*input_)[index];
326 if ((border_radius_ > 0.0) && (
pcl::isFinite(current_point)))
329 std::vector<float> nn_distances;
331 this->searchForNeighbors (index, border_radius_, nn_indices, nn_distances);
333 for (
const auto &nn_index : nn_indices)
335 if (edge_points_[nn_index])
337 borders[index] =
true;
345 Eigen::Vector3d *omp_mem =
new Eigen::Vector3d[threads_];
347 for (std::size_t i = 0; i < threads_; i++)
348 omp_mem[i].setZero (3);
350 Eigen::Vector3d *omp_mem =
new Eigen::Vector3d[1];
352 omp_mem[0].setZero (3);
355 double *prg_local_mem =
new double[input_->size () * 3];
356 double **prg_mem =
new double * [input_->size ()];
358 for (std::size_t i = 0; i < input_->size (); i++)
359 prg_mem[i] = prg_local_mem + 3 * i;
361 #pragma omp parallel for \
363 shared(borders, omp_mem, prg_mem) \
364 num_threads(threads_)
365 for (
int index = 0; index < static_cast<int> (input_->size ()); index++)
368 int tid = omp_get_thread_num ();
372 PointInT current_point = (*input_)[index];
377 Eigen::Matrix3d cov_m = Eigen::Matrix3d::Zero ();
378 getScatterMatrix (
static_cast<int> (index), cov_m);
380 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver (cov_m);
382 const double& e1c = solver.eigenvalues ()[2];
383 const double& e2c = solver.eigenvalues ()[1];
384 const double& e3c = solver.eigenvalues ()[0];
386 if (!std::isfinite (e1c) || !std::isfinite (e2c) || !std::isfinite (e3c))
391 PCL_WARN (
"[pcl::%s::detectKeypoints] : The third eigenvalue is negative! Skipping the point with index %i.\n",
392 name_.c_str (), index);
396 omp_mem[tid][0] = e2c / e1c;
397 omp_mem[tid][1] = e3c / e2c;
398 omp_mem[tid][2] = e3c;
401 for (Eigen::Index d = 0; d < omp_mem[tid].size (); d++)
402 prg_mem[index][d] = omp_mem[tid][d];
405 for (
int index = 0; index < int (input_->size ()); index++)
409 if ((prg_mem[index][0] < gamma_21_) && (prg_mem[index][1] < gamma_32_))
410 third_eigen_value_[index] = prg_mem[index][2];
414 bool* feat_max =
new bool [input_->size()];
416 #pragma omp parallel for \
419 num_threads(threads_)
420 for (
int index = 0; index < int (input_->size ()); index++)
422 feat_max [index] =
false;
423 PointInT current_point = (*input_)[index];
425 if ((third_eigen_value_[index] > 0.0) && (
pcl::isFinite(current_point)))
428 std::vector<float> nn_distances;
431 this->searchForNeighbors (index, non_max_radius_, nn_indices, nn_distances);
433 n_neighbors =
static_cast<int> (nn_indices.size ());
435 if (n_neighbors >= min_neighbors_)
439 for (
const auto& j : nn_indices)
440 if (third_eigen_value_[index] < third_eigen_value_[j])
443 feat_max[index] =
true;
448 #pragma omp parallel for \
450 shared(feat_max, output) \
451 num_threads(threads_)
452 for (
int index = 0; index < int (input_->size ()); index++)
458 p.getVector3fMap () = (*input_)[index].getVector3fMap ();
460 keypoints_indices_->indices.push_back (index);
464 output.header = input_->header;
465 output.width = output.size ();
469 if (border_radius_ > 0.0)
474 delete[] prg_local_mem;
479 #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
void setNumberOfThreads(unsigned int nr_threads=0)
Initialize the scheduler and set the number of threads to use.
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.