38 #ifndef PCL_ISS_KEYPOINT3D_IMPL_H_
39 #define PCL_ISS_KEYPOINT3D_IMPL_H_
41 #include <pcl/features/boundary.h>
42 #include <pcl/features/normal_3d.h>
43 #include <pcl/features/integral_image_normal.h>
45 #include <pcl/keypoints/iss_3d.h>
48 template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
51 salient_radius_ = salient_radius;
55 template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
58 non_max_radius_ = non_max_radius;
62 template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
65 normal_radius_ = normal_radius;
69 template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
72 border_radius_ = border_radius;
76 template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
83 template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
90 template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
93 min_neighbors_ = min_neighbors;
97 template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
104 template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
bool*
107 bool* edge_points =
new bool [input.size ()];
109 Eigen::Vector4f u = Eigen::Vector4f::Zero ();
110 Eigen::Vector4f v = Eigen::Vector4f::Zero ();
115 #pragma omp parallel for \
117 shared(angle_threshold, boundary_estimator, border_radius, edge_points, input) \
119 num_threads(threads_)
120 for (
int index = 0; index < int (input.points.size ()); index++)
122 edge_points[index] =
false;
123 PointInT current_point = input.points[index];
127 std::vector<int> nn_indices;
128 std::vector<float> nn_distances;
131 this->searchForNeighbors (
static_cast<int> (index), border_radius, nn_indices, nn_distances);
133 n_neighbors =
static_cast<int> (nn_indices.size ());
135 if (n_neighbors >= min_neighbors_)
139 if (boundary_estimator.
isBoundaryPoint (input,
static_cast<int> (index), nn_indices, u, v, angle_threshold))
140 edge_points[index] =
true;
145 return (edge_points);
149 template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
152 const PointInT& current_point = (*input_).points[current_index];
154 double central_point[3];
155 memset(central_point, 0,
sizeof(
double) * 3);
157 central_point[0] = current_point.x;
158 central_point[1] = current_point.y;
159 central_point[2] = current_point.z;
161 cov_m = Eigen::Matrix3d::Zero ();
163 std::vector<int> nn_indices;
164 std::vector<float> nn_distances;
167 this->searchForNeighbors (current_index, salient_radius_, nn_indices, nn_distances);
169 n_neighbors =
static_cast<int> (nn_indices.size ());
171 if (n_neighbors < min_neighbors_)
175 memset(cov, 0,
sizeof(
double) * 9);
177 for (
int n_idx = 0; n_idx < n_neighbors; n_idx++)
179 const PointInT& n_point = (*input_).points[nn_indices[n_idx]];
181 double neigh_point[3];
182 memset(neigh_point, 0,
sizeof(
double) * 3);
184 neigh_point[0] = n_point.x;
185 neigh_point[1] = n_point.y;
186 neigh_point[2] = n_point.z;
188 for (
int i = 0; i < 3; i++)
189 for (
int j = 0; j < 3; j++)
190 cov[i * 3 + j] += (neigh_point[i] - central_point[i]) * (neigh_point[j] - central_point[j]);
193 cov_m << cov[0], cov[1], cov[2],
194 cov[3], cov[4], cov[5],
195 cov[6], cov[7], cov[8];
199 template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
bool
204 PCL_ERROR (
"[pcl::%s::initCompute] init failed!\n", name_.c_str ());
207 if (salient_radius_ <= 0)
209 PCL_ERROR (
"[pcl::%s::initCompute] : the salient radius (%f) must be strict positive!\n",
210 name_.c_str (), salient_radius_);
213 if (non_max_radius_ <= 0)
215 PCL_ERROR (
"[pcl::%s::initCompute] : the non maxima radius (%f) must be strict positive!\n",
216 name_.c_str (), non_max_radius_);
221 PCL_ERROR (
"[pcl::%s::initCompute] : the threshold on the ratio between the 2nd and the 1rst eigenvalue (%f) must be strict positive!\n",
222 name_.c_str (), gamma_21_);
227 PCL_ERROR (
"[pcl::%s::initCompute] : the threshold on the ratio between the 3rd and the 2nd eigenvalue (%f) must be strict positive!\n",
228 name_.c_str (), gamma_32_);
231 if (min_neighbors_ <= 0)
233 PCL_ERROR (
"[pcl::%s::initCompute] : the minimum number of neighbors (%f) must be strict positive!\n",
234 name_.c_str (), min_neighbors_);
238 delete[] third_eigen_value_;
240 third_eigen_value_ =
new double[input_->size ()];
241 memset(third_eigen_value_, 0,
sizeof(
double) * input_->size ());
243 delete[] edge_points_;
245 if (border_radius_ > 0.0)
247 if (normals_->empty ())
249 if (normal_radius_ <= 0.)
251 PCL_ERROR (
"[pcl::%s::initCompute] : the radius used to estimate surface normals (%f) must be positive!\n",
252 name_.c_str (), normal_radius_);
257 if (input_->height == 1 )
262 normal_estimation.
compute (*normal_ptr);
270 normal_estimation.
compute (*normal_ptr);
272 normals_ = normal_ptr;
274 if (normals_->size () != surface_->size ())
276 PCL_ERROR (
"[pcl::%s::initCompute] normals given, but the number of normals does not match the number of input points!\n", name_.c_str ());
280 else if (border_radius_ < 0.0)
282 PCL_ERROR (
"[pcl::%s::initCompute] : the border radius used to estimate boundary points (%f) must be positive!\n",
283 name_.c_str (), border_radius_);
291 template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
295 output.points.clear ();
297 if (border_radius_ > 0.0)
298 edge_points_ = getBoundaryPoints (*(input_->makeShared ()), border_radius_, angle_threshold_);
300 bool* borders =
new bool [input_->size()];
302 #pragma omp parallel for \
305 num_threads(threads_)
306 for (
int index = 0; index < int (input_->size ()); index++)
308 borders[index] =
false;
309 PointInT current_point = input_->points[index];
311 if ((border_radius_ > 0.0) && (
pcl::isFinite(current_point)))
313 std::vector<int> nn_indices;
314 std::vector<float> nn_distances;
316 this->searchForNeighbors (
static_cast<int> (index), border_radius_, nn_indices, nn_distances);
318 for (
const int &nn_index : nn_indices)
320 if (edge_points_[nn_index])
322 borders[index] =
true;
330 Eigen::Vector3d *omp_mem =
new Eigen::Vector3d[threads_];
332 for (std::size_t i = 0; i < threads_; i++)
333 omp_mem[i].setZero (3);
335 Eigen::Vector3d *omp_mem =
new Eigen::Vector3d[1];
337 omp_mem[0].setZero (3);
340 double *prg_local_mem =
new double[input_->size () * 3];
341 double **prg_mem =
new double * [input_->size ()];
343 for (std::size_t i = 0; i < input_->size (); i++)
344 prg_mem[i] = prg_local_mem + 3 * i;
346 #pragma omp parallel for \
348 shared(borders, omp_mem, prg_mem) \
349 num_threads(threads_)
350 for (
int index = 0; index < static_cast<int> (input_->size ()); index++)
353 int tid = omp_get_thread_num ();
357 PointInT current_point = input_->points[index];
362 Eigen::Matrix3d cov_m = Eigen::Matrix3d::Zero ();
363 getScatterMatrix (
static_cast<int> (index), cov_m);
365 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver (cov_m);
367 const double& e1c = solver.eigenvalues ()[2];
368 const double& e2c = solver.eigenvalues ()[1];
369 const double& e3c = solver.eigenvalues ()[0];
371 if (!std::isfinite (e1c) || !std::isfinite (e2c) || !std::isfinite (e3c))
376 PCL_WARN (
"[pcl::%s::detectKeypoints] : The third eigenvalue is negative! Skipping the point with index %i.\n",
377 name_.c_str (), index);
381 omp_mem[tid][0] = e2c / e1c;
382 omp_mem[tid][1] = e3c / e2c;;
383 omp_mem[tid][2] = e3c;
386 for (Eigen::Index d = 0; d < omp_mem[tid].size (); d++)
387 prg_mem[index][d] = omp_mem[tid][d];
390 for (
int index = 0; index < int (input_->size ()); index++)
394 if ((prg_mem[index][0] < gamma_21_) && (prg_mem[index][1] < gamma_32_))
395 third_eigen_value_[index] = prg_mem[index][2];
399 bool* feat_max =
new bool [input_->size()];
401 #pragma omp parallel for \
404 num_threads(threads_)
405 for (
int index = 0; index < int (input_->size ()); index++)
407 feat_max [index] =
false;
408 PointInT current_point = input_->points[index];
410 if ((third_eigen_value_[index] > 0.0) && (
pcl::isFinite(current_point)))
412 std::vector<int> nn_indices;
413 std::vector<float> nn_distances;
416 this->searchForNeighbors (
static_cast<int> (index), non_max_radius_, nn_indices, nn_distances);
418 n_neighbors =
static_cast<int> (nn_indices.size ());
420 if (n_neighbors >= min_neighbors_)
424 for (
int j = 0 ; j < n_neighbors; j++)
425 if (third_eigen_value_[index] < third_eigen_value_[nn_indices[j]])
428 feat_max[index] =
true;
433 #pragma omp parallel for \
435 shared(feat_max, output) \
436 num_threads(threads_)
437 for (
int index = 0; index < int (input_->size ()); index++)
443 p.getVector3fMap () = input_->points[index].getVector3fMap ();
444 output.points.push_back(p);
445 keypoints_indices_->indices.push_back (index);
449 output.header = input_->header;
450 output.width =
static_cast<std::uint32_t> (output.points.size ());
454 if (border_radius_ > 0.0)
459 delete[] prg_local_mem;
464 #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 std::vector< int > &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...