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 (
size_t 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 (
int j = 0 ; j < nn_indices.size (); j++)
321 if (edge_points_[nn_indices[j]])
323 borders[index] =
true;
330 Eigen::Vector3d *omp_mem =
new Eigen::Vector3d[threads_];
332 for (
int i = 0; i < threads_; i++)
333 omp_mem[i].setZero (3);
335 double *prg_local_mem =
new double[input_->size () * 3];
336 double **prg_mem =
new double * [input_->size ()];
338 for (
int i = 0; i < input_->size (); i++)
339 prg_mem[i] = prg_local_mem + 3 * i;
342 #pragma omp parallel for num_threads(threads_)
344 for (index = 0; index < static_cast<int> (input_->size ()); index++)
347 int tid = omp_get_thread_num ();
351 PointInT current_point = input_->points[index];
356 Eigen::Matrix3d cov_m = Eigen::Matrix3d::Zero ();
357 getScatterMatrix (static_cast<int> (index), cov_m);
359 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver (cov_m);
361 const double& e1c = solver.eigenvalues ()[2];
362 const double& e2c = solver.eigenvalues ()[1];
363 const double& e3c = solver.eigenvalues ()[0];
365 if (!pcl_isfinite (e1c) || !pcl_isfinite (e2c) || !pcl_isfinite (e3c))
370 PCL_WARN (
"[pcl::%s::detectKeypoints] : The third eigenvalue is negative! Skipping the point with index %i.\n",
371 name_.c_str (), index);
375 omp_mem[tid][0] = e2c / e1c;
376 omp_mem[tid][1] = e3c / e2c;;
377 omp_mem[tid][2] = e3c;
380 for (
int d = 0; d < omp_mem[tid].size (); d++)
381 prg_mem[index][d] = omp_mem[tid][d];
384 for (index = 0; index < int (input_->size ()); index++)
388 if ((prg_mem[index][0] < gamma_21_) && (prg_mem[index][1] < gamma_32_))
389 third_eigen_value_[index] = prg_mem[index][2];
393 bool* feat_max =
new bool [input_->size()];
397 #pragma omp parallel for private(is_max) num_threads(threads_)
399 for (index = 0; index < int (input_->size ()); index++)
401 feat_max [index] =
false;
402 PointInT current_point = input_->points[index];
404 if ((third_eigen_value_[index] > 0.0) && (
pcl::isFinite(current_point)))
406 std::vector<int> nn_indices;
407 std::vector<float> nn_distances;
410 this->searchForNeighbors (static_cast<int> (index), non_max_radius_, nn_indices, nn_distances);
412 n_neighbors =
static_cast<int> (nn_indices.size ());
414 if (n_neighbors >= min_neighbors_)
418 for (
size_t j = 0 ; j < n_neighbors; j++)
419 if (third_eigen_value_[index] < third_eigen_value_[nn_indices[j]])
422 feat_max[index] =
true;
428 #pragma omp parallel for shared (output) num_threads(threads_)
430 for (index = 0; index < int (input_->size ()); index++)
436 output.
points.push_back(input_->points[index]);
439 output.
header = input_->header;
440 output.
width =
static_cast<uint32_t
> (output.
points.size ());
444 if (border_radius_ > 0.0)
449 delete[] prg_local_mem;
453 #define PCL_INSTANTIATE_ISSKeypoint3D(T,U,N) template class PCL_EXPORTS pcl::ISSKeypoint3D<T,U,N>;
void setNormalEstimationMethod(NormalEstimationMethod normal_estimation_method)
Set the normal estimation method.
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point...
void setBorderRadius(double border_radius)
Set the radius used for the estimation of the boundary points.
void detectKeypoints(PointCloudOut &output)
Detect the keypoints by performing the EVD of the scatter matrix.
pcl::PCLHeader header
The point cloud header.
PointCloudN::Ptr PointCloudNPtr
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
void setNormals(const PointCloudNConstPtr &normals)
Set the normals if pre-calculated normals are available.
uint32_t height
The point cloud height (if organized as an image-structure).
bool initCompute()
Perform the initial checks before computing the keypoints.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
void setNonMaxRadius(double non_max_radius)
Set the radius for the application of the non maxima supression algorithm.
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
BoundaryEstimation estimates whether a set of points is lying on surface boundaries using an angle cr...
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested.
bool * getBoundaryPoints(PointCloudIn &input, double border_radius, float angle_threshold)
Compute the boundary points for the given input cloud.
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
virtual void setInputCloud(const typename PointCloudIn::ConstPtr &cloud)
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) ...
Surface normal estimation on organized data using integral images.
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...
Keypoint represents the base class for key points.
void setNormalRadius(double normal_radius)
Set the radius used for the estimation of the surface normals of the input cloud. ...
void setSalientRadius(double salient_radius)
Set the radius of the spherical neighborhood used to compute the scatter matrix.
PointCloudN::ConstPtr PointCloudNConstPtr
void getScatterMatrix(const int ¤t_index, Eigen::Matrix3d &cov_m)
Compute the scatter matrix for a point index.
uint32_t width
The point cloud width (if organized as an image-structure).
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.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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 setThreshold32(double gamma_32)
Set the upper bound on the ratio between the third and the second eigenvalue.