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>; void setThreshold32(double gamma_32)
Set the upper bound on the ratio between the third and the second eigenvalue.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
void setSalientRadius(double salient_radius)
Set the radius of the spherical neighborhood used to compute the scatter matrix.
void getScatterMatrix(const int ¤t_index, Eigen::Matrix3d &cov_m)
Compute the scatter matrix for a point index.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void setBorderRadius(double border_radius)
Set the radius used for the estimation of the boundary points.
BoundaryEstimation estimates whether a set of points is lying on surface boundaries using an angle cr...
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.
void setNormals(const PointCloudNConstPtr &normals)
Set the normals if pre-calculated normals are available.
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point.
void setThreshold21(double gamma_21)
Set the upper bound on the ratio between the second and the first eigenvalue.
uint32_t height
The point cloud height (if organized as an image-structure).
void setMinNeighbors(int min_neighbors)
Set the minimum number of neighbors that has to be found while applying the non maxima suppression al...
Keypoint represents the base class for key points.
void detectKeypoints(PointCloudOut &output)
Detect the keypoints by performing the EVD of the scatter matrix.
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.
uint32_t width
The point cloud width (if organized as an image-structure).
bool * getBoundaryPoints(PointCloudIn &input, double border_radius, float angle_threshold)
Compute the boundary points for the given input cloud.
Surface normal estimation on organized data using integral images.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
void setNormalRadius(double normal_radius)
Set the radius used for the estimation of the surface normals of the input cloud.
void setNormalEstimationMethod(NormalEstimationMethod normal_estimation_method)
Set the normal estimation method.
pcl::PCLHeader header
The point cloud header.
PointCloudN::ConstPtr PointCloudNConstPtr
virtual void setInputCloud(const typename PointCloudIn::ConstPtr &cloud)
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
void setNonMaxRadius(double non_max_radius)
Set the radius for the application of the non maxima supression algorithm.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
bool initCompute()
Perform the initial checks before computing the keypoints.
PointCloudN::Ptr PointCloudNPtr
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.