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>;