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 ();
117 #pragma omp parallel for private(u, v) num_threads(threads_)
119 for (index = 0; index < int (input.
points.size ()); index++)
121 edge_points[index] =
false;
122 PointInT current_point = input.
points[index];
126 std::vector<int> nn_indices;
127 std::vector<float> nn_distances;
130 this->searchForNeighbors (
static_cast<int> (index), border_radius, nn_indices, nn_distances);
132 n_neighbors =
static_cast<int> (nn_indices.size ());
134 if (n_neighbors >= min_neighbors_)
138 if (boundary_estimator.
isBoundaryPoint (input,
static_cast<int> (index), nn_indices, u, v, angle_threshold))
139 edge_points[index] =
true;
144 return (edge_points);
148 template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
151 const PointInT& current_point = (*input_).points[current_index];
153 double central_point[3];
154 memset(central_point, 0,
sizeof(
double) * 3);
156 central_point[0] = current_point.x;
157 central_point[1] = current_point.y;
158 central_point[2] = current_point.z;
160 cov_m = Eigen::Matrix3d::Zero ();
162 std::vector<int> nn_indices;
163 std::vector<float> nn_distances;
166 this->searchForNeighbors (current_index, salient_radius_, nn_indices, nn_distances);
168 n_neighbors =
static_cast<int> (nn_indices.size ());
170 if (n_neighbors < min_neighbors_)
174 memset(cov, 0,
sizeof(
double) * 9);
176 for (
int n_idx = 0; n_idx < n_neighbors; n_idx++)
178 const PointInT& n_point = (*input_).points[nn_indices[n_idx]];
180 double neigh_point[3];
181 memset(neigh_point, 0,
sizeof(
double) * 3);
183 neigh_point[0] = n_point.x;
184 neigh_point[1] = n_point.y;
185 neigh_point[2] = n_point.z;
187 for (
int i = 0; i < 3; i++)
188 for (
int j = 0; j < 3; j++)
189 cov[i * 3 + j] += (neigh_point[i] - central_point[i]) * (neigh_point[j] - central_point[j]);
192 cov_m << cov[0], cov[1], cov[2],
193 cov[3], cov[4], cov[5],
194 cov[6], cov[7], cov[8];
198 template<
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
bool
203 PCL_ERROR (
"[pcl::%s::initCompute] init failed!\n", name_.c_str ());
206 if (salient_radius_ <= 0)
208 PCL_ERROR (
"[pcl::%s::initCompute] : the salient radius (%f) must be strict positive!\n",
209 name_.c_str (), salient_radius_);
212 if (non_max_radius_ <= 0)
214 PCL_ERROR (
"[pcl::%s::initCompute] : the non maxima radius (%f) must be strict positive!\n",
215 name_.c_str (), non_max_radius_);
220 PCL_ERROR (
"[pcl::%s::initCompute] : the threshold on the ratio between the 2nd and the 1rst eigenvalue (%f) must be strict positive!\n",
221 name_.c_str (), gamma_21_);
226 PCL_ERROR (
"[pcl::%s::initCompute] : the threshold on the ratio between the 3rd and the 2nd eigenvalue (%f) must be strict positive!\n",
227 name_.c_str (), gamma_32_);
230 if (min_neighbors_ <= 0)
232 PCL_ERROR (
"[pcl::%s::initCompute] : the minimum number of neighbors (%f) must be strict positive!\n",
233 name_.c_str (), min_neighbors_);
237 if (third_eigen_value_)
238 delete[] third_eigen_value_;
240 third_eigen_value_ =
new double[input_->size ()];
241 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()];
305 #pragma omp parallel for num_threads(threads_)
307 for (index = 0; index < int (input_->size ()); index++)
309 borders[index] =
false;
310 PointInT current_point = input_->points[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 (
size_t j = 0 ; j < nn_indices.size (); j++)
321 if (edge_points_[nn_indices[j]])
323 borders[index] =
true;
331 Eigen::Vector3d *omp_mem =
new Eigen::Vector3d[threads_];
333 for (
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 (
size_t i = 0; i < input_->size (); i++)
345 prg_mem[i] = prg_local_mem + 3 * i;
348 #pragma omp parallel for num_threads(threads_)
350 for (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 (!pcl_isfinite (e1c) || !pcl_isfinite (e2c) || !pcl_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 (
int d = 0; d < omp_mem[tid].size (); d++)
387 prg_mem[index][d] = omp_mem[tid][d];
390 for (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()];
403 #pragma omp parallel for private(is_max) num_threads(threads_)
405 for (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;
434 #pragma omp parallel for shared (output) num_threads(threads_)
436 for (index = 0; index < int (input_->size ()); index++)
444 p.getVector3fMap () = input_->points[index].getVector3fMap ();
445 output.
points.push_back(p);
446 keypoints_indices_->indices.push_back (index);
450 output.
header = input_->header;
451 output.
width =
static_cast<uint32_t
> (output.
points.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>;