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];
128 std::vector<int> nn_indices;
129 std::vector<float> nn_distances;
132 this->searchForNeighbors (
static_cast<int> (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 ();
164 std::vector<int> nn_indices;
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 (
int n_idx = 0; n_idx < n_neighbors; n_idx++)
180 const PointInT& n_point = (*input_)[nn_indices[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)))
314 std::vector<int> nn_indices;
315 std::vector<float> nn_distances;
317 this->searchForNeighbors (
static_cast<int> (index), border_radius_, nn_indices, nn_distances);
319 for (
const int &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)))
413 std::vector<int> nn_indices;
414 std::vector<float> nn_distances;
417 this->searchForNeighbors (
static_cast<int> (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 (
int j = 0 ; j < n_neighbors; j++)
426 if (third_eigen_value_[index] < third_eigen_value_[nn_indices[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>;