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 ();
116 #pragma omp parallel for private(u, v) num_threads(threads_)
118 for (
int index = 0; index < int (input.points.size ()); index++)
120 edge_points[index] =
false;
121 PointInT current_point = input.points[index];
125 std::vector<int> nn_indices;
126 std::vector<float> nn_distances;
129 this->searchForNeighbors (static_cast<int> (index), border_radius, nn_indices, nn_distances);
131 n_neighbors = static_cast<int> (nn_indices.size ());
133 if (n_neighbors >= min_neighbors_)
137 if (boundary_estimator.
isBoundaryPoint (input, static_cast<int> (index), nn_indices, u, v, angle_threshold))
138 edge_points[index] =
true;
143 return (edge_points);
147 template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
150 const PointInT& current_point = (*input_).points[current_index];
152 double central_point[3];
153 memset(central_point, 0,
sizeof(
double) * 3);
155 central_point[0] = current_point.x;
156 central_point[1] = current_point.y;
157 central_point[2] = current_point.z;
159 cov_m = Eigen::Matrix3d::Zero ();
161 std::vector<int> nn_indices;
162 std::vector<float> nn_distances;
165 this->searchForNeighbors (current_index, salient_radius_, nn_indices, nn_distances);
167 n_neighbors = static_cast<int> (nn_indices.size ());
169 if (n_neighbors < min_neighbors_)
173 memset(cov, 0,
sizeof(
double) * 9);
175 for (
int n_idx = 0; n_idx < n_neighbors; n_idx++)
177 const PointInT& n_point = (*input_).points[nn_indices[n_idx]];
179 double neigh_point[3];
180 memset(neigh_point, 0,
sizeof(
double) * 3);
182 neigh_point[0] = n_point.x;
183 neigh_point[1] = n_point.y;
184 neigh_point[2] = n_point.z;
186 for (
int i = 0; i < 3; i++)
187 for (
int j = 0; j < 3; j++)
188 cov[i * 3 + j] += (neigh_point[i] - central_point[i]) * (neigh_point[j] - central_point[j]);
191 cov_m << cov[0], cov[1], cov[2],
192 cov[3], cov[4], cov[5],
193 cov[6], cov[7], cov[8];
197 template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
bool
202 PCL_ERROR (
"[pcl::%s::initCompute] init failed!\n", name_.c_str ());
205 if (salient_radius_ <= 0)
207 PCL_ERROR (
"[pcl::%s::initCompute] : the salient radius (%f) must be strict positive!\n",
208 name_.c_str (), salient_radius_);
211 if (non_max_radius_ <= 0)
213 PCL_ERROR (
"[pcl::%s::initCompute] : the non maxima radius (%f) must be strict positive!\n",
214 name_.c_str (), non_max_radius_);
219 PCL_ERROR (
"[pcl::%s::initCompute] : the threshold on the ratio between the 2nd and the 1rst eigenvalue (%f) must be strict positive!\n",
220 name_.c_str (), gamma_21_);
225 PCL_ERROR (
"[pcl::%s::initCompute] : the threshold on the ratio between the 3rd and the 2nd eigenvalue (%f) must be strict positive!\n",
226 name_.c_str (), gamma_32_);
229 if (min_neighbors_ <= 0)
231 PCL_ERROR (
"[pcl::%s::initCompute] : the minimum number of neighbors (%f) must be strict positive!\n",
232 name_.c_str (), min_neighbors_);
236 if (third_eigen_value_)
237 delete[] third_eigen_value_;
239 third_eigen_value_ =
new double[input_->size ()];
240 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()];
303 #pragma omp parallel for num_threads(threads_)
305 for (
int index = 0; index < int (input_->size ()); index++)
307 borders[index] =
false;
308 PointInT current_point = input_->points[index];
310 if ((border_radius_ > 0.0) && (
pcl::isFinite(current_point)))
312 std::vector<int> nn_indices;
313 std::vector<float> nn_distances;
315 this->searchForNeighbors (static_cast<int> (index), border_radius_, nn_indices, nn_distances);
317 for (
const int &nn_index : nn_indices)
319 if (edge_points_[nn_index])
321 borders[index] =
true;
329 Eigen::Vector3d *omp_mem =
new Eigen::Vector3d[threads_];
331 for (std::size_t i = 0; i < threads_; i++)
332 omp_mem[i].setZero (3);
334 Eigen::Vector3d *omp_mem =
new Eigen::Vector3d[1];
336 omp_mem[0].setZero (3);
339 double *prg_local_mem =
new double[input_->size () * 3];
340 double **prg_mem =
new double * [input_->size ()];
342 for (std::size_t i = 0; i < input_->size (); i++)
343 prg_mem[i] = prg_local_mem + 3 * i;
346 #pragma omp parallel for num_threads(threads_)
348 for (
int index = 0; index < static_cast<int> (input_->size ()); index++)
351 int tid = omp_get_thread_num ();
355 PointInT current_point = input_->points[index];
360 Eigen::Matrix3d cov_m = Eigen::Matrix3d::Zero ();
361 getScatterMatrix (static_cast<int> (index), cov_m);
363 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver (cov_m);
365 const double& e1c = solver.eigenvalues ()[2];
366 const double& e2c = solver.eigenvalues ()[1];
367 const double& e3c = solver.eigenvalues ()[0];
369 if (!std::isfinite (e1c) || !std::isfinite (e2c) || !std::isfinite (e3c))
374 PCL_WARN (
"[pcl::%s::detectKeypoints] : The third eigenvalue is negative! Skipping the point with index %i.\n",
375 name_.c_str (), index);
379 omp_mem[tid][0] = e2c / e1c;
380 omp_mem[tid][1] = e3c / e2c;;
381 omp_mem[tid][2] = e3c;
384 for (Eigen::Index d = 0; d < omp_mem[tid].size (); d++)
385 prg_mem[index][d] = omp_mem[tid][d];
388 for (
int index = 0; index < int (input_->size ()); index++)
392 if ((prg_mem[index][0] < gamma_21_) && (prg_mem[index][1] < gamma_32_))
393 third_eigen_value_[index] = prg_mem[index][2];
397 bool* feat_max =
new bool [input_->size()];
401 #pragma omp parallel for private(is_max) num_threads(threads_)
403 for (
int index = 0; index < int (input_->size ()); index++)
405 feat_max [index] =
false;
406 PointInT current_point = input_->points[index];
408 if ((third_eigen_value_[index] > 0.0) && (
pcl::isFinite(current_point)))
410 std::vector<int> nn_indices;
411 std::vector<float> nn_distances;
414 this->searchForNeighbors (static_cast<int> (index), non_max_radius_, nn_indices, nn_distances);
416 n_neighbors = static_cast<int> (nn_indices.size ());
418 if (n_neighbors >= min_neighbors_)
422 for (
int j = 0 ; j < n_neighbors; j++)
423 if (third_eigen_value_[index] < third_eigen_value_[nn_indices[j]])
426 feat_max[index] =
true;
432 #pragma omp parallel for shared (output) num_threads(threads_)
434 for (
int index = 0; index < int (input_->size ()); index++)
442 p.getVector3fMap () = input_->points[index].getVector3fMap ();
443 output.points.push_back(p);
444 keypoints_indices_->indices.push_back (index);
448 output.header = input_->header;
449 output.width = static_cast<std::uint32_t> (output.points.size ());
453 if (border_radius_ > 0.0)
458 delete[] prg_local_mem;
463 #define PCL_INSTANTIATE_ISSKeypoint3D(T,U,N) template class PCL_EXPORTS pcl::ISSKeypoint3D<T,U,N>;