Point Cloud Library (PCL) 1.12.1
iss_3d.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2011, Willow Garage, Inc.
6 *
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of Willow Garage, Inc. nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *
36 */
37
38#ifndef PCL_ISS_KEYPOINT3D_IMPL_H_
39#define PCL_ISS_KEYPOINT3D_IMPL_H_
40
41#include <Eigen/Eigenvalues> // for SelfAdjointEigenSolver
42#include <pcl/features/boundary.h>
43#include <pcl/features/normal_3d.h>
44#include <pcl/features/integral_image_normal.h>
45
46#include <pcl/keypoints/iss_3d.h>
47
48//////////////////////////////////////////////////////////////////////////////////////////////
49template<typename PointInT, typename PointOutT, typename NormalT> void
51{
52 salient_radius_ = salient_radius;
53}
54
55//////////////////////////////////////////////////////////////////////////////////////////////
56template<typename PointInT, typename PointOutT, typename NormalT> void
58{
59 non_max_radius_ = non_max_radius;
60}
61
62//////////////////////////////////////////////////////////////////////////////////////////////
63template<typename PointInT, typename PointOutT, typename NormalT> void
65{
66 normal_radius_ = normal_radius;
67}
68
69//////////////////////////////////////////////////////////////////////////////////////////////
70template<typename PointInT, typename PointOutT, typename NormalT> void
72{
73 border_radius_ = border_radius;
74}
75
76//////////////////////////////////////////////////////////////////////////////////////////////
77template<typename PointInT, typename PointOutT, typename NormalT> void
79{
80 gamma_21_ = gamma_21;
81}
82
83//////////////////////////////////////////////////////////////////////////////////////////////
84template<typename PointInT, typename PointOutT, typename NormalT> void
86{
87 gamma_32_ = gamma_32;
88}
89
90//////////////////////////////////////////////////////////////////////////////////////////////
91template<typename PointInT, typename PointOutT, typename NormalT> void
93{
94 min_neighbors_ = min_neighbors;
95}
96
97//////////////////////////////////////////////////////////////////////////////////////////////
98template<typename PointInT, typename PointOutT, typename NormalT> void
100{
101 normals_ = normals;
102}
103
104//////////////////////////////////////////////////////////////////////////////////////////////
105template<typename PointInT, typename PointOutT, typename NormalT> void
107{
108 if (nr_threads == 0)
109#ifdef _OPENMP
110 threads_ = omp_get_num_procs();
111#else
112 threads_ = 1;
113#endif
114 else
115 threads_ = nr_threads;
116}
117
118//////////////////////////////////////////////////////////////////////////////////////////////
119template<typename PointInT, typename PointOutT, typename NormalT> bool*
121{
122 bool* edge_points = new bool [input.size ()];
123
124 Eigen::Vector4f u = Eigen::Vector4f::Zero ();
125 Eigen::Vector4f v = Eigen::Vector4f::Zero ();
126
128 boundary_estimator.setInputCloud (input_);
129
130#pragma omp parallel for \
131 default(none) \
132 shared(angle_threshold, boundary_estimator, border_radius, edge_points, input) \
133 firstprivate(u, v) \
134 num_threads(threads_)
135 for (int index = 0; index < int (input.size ()); index++)
136 {
137 edge_points[index] = false;
138 PointInT current_point = input[index];
139
140 if (pcl::isFinite(current_point))
141 {
142 pcl::Indices nn_indices;
143 std::vector<float> nn_distances;
144 int n_neighbors;
145
146 this->searchForNeighbors (index, border_radius, nn_indices, nn_distances);
147
148 n_neighbors = static_cast<int> (nn_indices.size ());
149
150 if (n_neighbors >= min_neighbors_)
151 {
152 boundary_estimator.getCoordinateSystemOnPlane ((*normals_)[index], u, v);
153
154 if (boundary_estimator.isBoundaryPoint (input, static_cast<int> (index), nn_indices, u, v, angle_threshold))
155 edge_points[index] = true;
156 }
157 }
158 }
159
160 return (edge_points);
161}
162
163//////////////////////////////////////////////////////////////////////////////////////////////
164template<typename PointInT, typename PointOutT, typename NormalT> void
165pcl::ISSKeypoint3D<PointInT, PointOutT, NormalT>::getScatterMatrix (const int& current_index, Eigen::Matrix3d &cov_m)
166{
167 const PointInT& current_point = (*input_)[current_index];
168
169 double central_point[3];
170 memset(central_point, 0, sizeof(double) * 3);
171
172 central_point[0] = current_point.x;
173 central_point[1] = current_point.y;
174 central_point[2] = current_point.z;
175
176 cov_m = Eigen::Matrix3d::Zero ();
177
178 pcl::Indices nn_indices;
179 std::vector<float> nn_distances;
180 int n_neighbors;
181
182 this->searchForNeighbors (current_index, salient_radius_, nn_indices, nn_distances);
183
184 n_neighbors = static_cast<int> (nn_indices.size ());
185
186 if (n_neighbors < min_neighbors_)
187 return;
188
189 double cov[9];
190 memset(cov, 0, sizeof(double) * 9);
191
192 for (const auto& n_idx : nn_indices)
193 {
194 const PointInT& n_point = (*input_)[n_idx];
195
196 double neigh_point[3];
197 memset(neigh_point, 0, sizeof(double) * 3);
198
199 neigh_point[0] = n_point.x;
200 neigh_point[1] = n_point.y;
201 neigh_point[2] = n_point.z;
202
203 for (int i = 0; i < 3; i++)
204 for (int j = 0; j < 3; j++)
205 cov[i * 3 + j] += (neigh_point[i] - central_point[i]) * (neigh_point[j] - central_point[j]);
206 }
207
208 cov_m << cov[0], cov[1], cov[2],
209 cov[3], cov[4], cov[5],
210 cov[6], cov[7], cov[8];
211}
212
213//////////////////////////////////////////////////////////////////////////////////////////////
214template<typename PointInT, typename PointOutT, typename NormalT> bool
216{
218 {
219 PCL_ERROR ("[pcl::%s::initCompute] init failed!\n", name_.c_str ());
220 return (false);
221 }
222 if (salient_radius_ <= 0)
223 {
224 PCL_ERROR ("[pcl::%s::initCompute] : the salient radius (%f) must be strict positive!\n",
225 name_.c_str (), salient_radius_);
226 return (false);
227 }
228 if (non_max_radius_ <= 0)
229 {
230 PCL_ERROR ("[pcl::%s::initCompute] : the non maxima radius (%f) must be strict positive!\n",
231 name_.c_str (), non_max_radius_);
232 return (false);
233 }
234 if (gamma_21_ <= 0)
235 {
236 PCL_ERROR ("[pcl::%s::initCompute] : the threshold on the ratio between the 2nd and the 1rst eigenvalue (%f) must be strict positive!\n",
237 name_.c_str (), gamma_21_);
238 return (false);
239 }
240 if (gamma_32_ <= 0)
241 {
242 PCL_ERROR ("[pcl::%s::initCompute] : the threshold on the ratio between the 3rd and the 2nd eigenvalue (%f) must be strict positive!\n",
243 name_.c_str (), gamma_32_);
244 return (false);
245 }
246 if (min_neighbors_ <= 0)
247 {
248 PCL_ERROR ("[pcl::%s::initCompute] : the minimum number of neighbors (%f) must be strict positive!\n",
249 name_.c_str (), min_neighbors_);
250 return (false);
251 }
252
253 delete[] third_eigen_value_;
254
255 third_eigen_value_ = new double[input_->size ()];
256 memset(third_eigen_value_, 0, sizeof(double) * input_->size ());
257
258 delete[] edge_points_;
259
260 if (border_radius_ > 0.0)
261 {
262 if (normals_->empty ())
263 {
264 if (normal_radius_ <= 0.)
265 {
266 PCL_ERROR ("[pcl::%s::initCompute] : the radius used to estimate surface normals (%f) must be positive!\n",
267 name_.c_str (), normal_radius_);
268 return (false);
269 }
270
271 PointCloudNPtr normal_ptr (new PointCloudN ());
272 if (input_->height == 1 )
273 {
275 normal_estimation.setInputCloud (surface_);
276 normal_estimation.setRadiusSearch (normal_radius_);
277 normal_estimation.compute (*normal_ptr);
278 }
279 else
280 {
283 normal_estimation.setInputCloud (surface_);
284 normal_estimation.setNormalSmoothingSize (5.0);
285 normal_estimation.compute (*normal_ptr);
286 }
287 normals_ = normal_ptr;
288 }
289 if (normals_->size () != surface_->size ())
290 {
291 PCL_ERROR ("[pcl::%s::initCompute] normals given, but the number of normals does not match the number of input points!\n", name_.c_str ());
292 return (false);
293 }
294 }
295 else if (border_radius_ < 0.0)
296 {
297 PCL_ERROR ("[pcl::%s::initCompute] : the border radius used to estimate boundary points (%f) must be positive!\n",
298 name_.c_str (), border_radius_);
299 return (false);
300 }
301
302 return (true);
303}
304
305//////////////////////////////////////////////////////////////////////////////////////////////
306template<typename PointInT, typename PointOutT, typename NormalT> void
308{
309 // Make sure the output cloud is empty
310 output.clear ();
311
312 if (border_radius_ > 0.0)
313 edge_points_ = getBoundaryPoints (*(input_->makeShared ()), border_radius_, angle_threshold_);
314
315 bool* borders = new bool [input_->size()];
316
317#pragma omp parallel for \
318 default(none) \
319 shared(borders) \
320 num_threads(threads_)
321 for (int index = 0; index < int (input_->size ()); index++)
322 {
323 borders[index] = false;
324 PointInT current_point = (*input_)[index];
325
326 if ((border_radius_ > 0.0) && (pcl::isFinite(current_point)))
327 {
328 pcl::Indices nn_indices;
329 std::vector<float> nn_distances;
330
331 this->searchForNeighbors (index, border_radius_, nn_indices, nn_distances);
332
333 for (const auto &nn_index : nn_indices)
334 {
335 if (edge_points_[nn_index])
336 {
337 borders[index] = true;
338 break;
339 }
340 }
341 }
342 }
343
344#ifdef _OPENMP
345 Eigen::Vector3d *omp_mem = new Eigen::Vector3d[threads_];
346
347 for (std::size_t i = 0; i < threads_; i++)
348 omp_mem[i].setZero (3);
349#else
350 Eigen::Vector3d *omp_mem = new Eigen::Vector3d[1];
351
352 omp_mem[0].setZero (3);
353#endif
354
355 double *prg_local_mem = new double[input_->size () * 3];
356 double **prg_mem = new double * [input_->size ()];
357
358 for (std::size_t i = 0; i < input_->size (); i++)
359 prg_mem[i] = prg_local_mem + 3 * i;
360
361#pragma omp parallel for \
362 default(none) \
363 shared(borders, omp_mem, prg_mem) \
364 num_threads(threads_)
365 for (int index = 0; index < static_cast<int> (input_->size ()); index++)
366 {
367#ifdef _OPENMP
368 int tid = omp_get_thread_num ();
369#else
370 int tid = 0;
371#endif
372 PointInT current_point = (*input_)[index];
373
374 if ((!borders[index]) && pcl::isFinite(current_point))
375 {
376 //if the considered point is not a border point and the point is "finite", then compute the scatter matrix
377 Eigen::Matrix3d cov_m = Eigen::Matrix3d::Zero ();
378 getScatterMatrix (static_cast<int> (index), cov_m);
379
380 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver (cov_m);
381
382 const double& e1c = solver.eigenvalues ()[2];
383 const double& e2c = solver.eigenvalues ()[1];
384 const double& e3c = solver.eigenvalues ()[0];
385
386 if (!std::isfinite (e1c) || !std::isfinite (e2c) || !std::isfinite (e3c))
387 continue;
388
389 if (e3c < 0)
390 {
391 PCL_WARN ("[pcl::%s::detectKeypoints] : The third eigenvalue is negative! Skipping the point with index %i.\n",
392 name_.c_str (), index);
393 continue;
394 }
395
396 omp_mem[tid][0] = e2c / e1c;
397 omp_mem[tid][1] = e3c / e2c;;
398 omp_mem[tid][2] = e3c;
399 }
400
401 for (Eigen::Index d = 0; d < omp_mem[tid].size (); d++)
402 prg_mem[index][d] = omp_mem[tid][d];
403 }
404
405 for (int index = 0; index < int (input_->size ()); index++)
406 {
407 if (!borders[index])
408 {
409 if ((prg_mem[index][0] < gamma_21_) && (prg_mem[index][1] < gamma_32_))
410 third_eigen_value_[index] = prg_mem[index][2];
411 }
412 }
413
414 bool* feat_max = new bool [input_->size()];
415
416#pragma omp parallel for \
417 default(none) \
418 shared(feat_max) \
419 num_threads(threads_)
420 for (int index = 0; index < int (input_->size ()); index++)
421 {
422 feat_max [index] = false;
423 PointInT current_point = (*input_)[index];
424
425 if ((third_eigen_value_[index] > 0.0) && (pcl::isFinite(current_point)))
426 {
427 pcl::Indices nn_indices;
428 std::vector<float> nn_distances;
429 int n_neighbors;
430
431 this->searchForNeighbors (index, non_max_radius_, nn_indices, nn_distances);
432
433 n_neighbors = static_cast<int> (nn_indices.size ());
434
435 if (n_neighbors >= min_neighbors_)
436 {
437 bool is_max = true;
438
439 for (const auto& j : nn_indices)
440 if (third_eigen_value_[index] < third_eigen_value_[j])
441 is_max = false;
442 if (is_max)
443 feat_max[index] = true;
444 }
445 }
446 }
447
448#pragma omp parallel for \
449 default(none) \
450 shared(feat_max, output) \
451 num_threads(threads_)
452 for (int index = 0; index < int (input_->size ()); index++)
453 {
454 if (feat_max[index])
455#pragma omp critical
456 {
457 PointOutT p;
458 p.getVector3fMap () = (*input_)[index].getVector3fMap ();
459 output.push_back(p);
460 keypoints_indices_->indices.push_back (index);
461 }
462 }
463
464 output.header = input_->header;
465 output.width = output.size ();
466 output.height = 1;
467
468 // Clear the contents of variables and arrays before the beginning of the next computation.
469 if (border_radius_ > 0.0)
470 normals_.reset (new pcl::PointCloud<NormalT>);
471
472 delete[] borders;
473 delete[] prg_mem;
474 delete[] prg_local_mem;
475 delete[] feat_max;
476 delete[] omp_mem;
477}
478
479#define PCL_INSTANTIATE_ISSKeypoint3D(T,U,N) template class PCL_EXPORTS pcl::ISSKeypoint3D<T,U,N>;
480
481#endif /* PCL_ISS_3D_IMPL_H_ */
BoundaryEstimation estimates whether a set of points is lying on surface boundaries using an angle cr...
Definition: boundary.h:80
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.
Definition: boundary.h:159
bool isBoundaryPoint(const pcl::PointCloud< PointInT > &cloud, int q_idx, const pcl::Indices &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.
Definition: boundary.hpp:51
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
Definition: feature.h:201
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition: feature.hpp:194
typename PointCloudN::ConstPtr PointCloudNConstPtr
Definition: iss_3d.h:96
void setNumberOfThreads(unsigned int nr_threads=0)
Initialize the scheduler and set the number of threads to use.
Definition: iss_3d.hpp:106
typename Keypoint< PointInT, PointOutT >::PointCloudIn PointCloudIn
Definition: iss_3d.h:91
void setNormals(const PointCloudNConstPtr &normals)
Set the normals if pre-calculated normals are available.
Definition: iss_3d.hpp:99
void setBorderRadius(double border_radius)
Set the radius used for the estimation of the boundary points.
Definition: iss_3d.hpp:71
void setSalientRadius(double salient_radius)
Set the radius of the spherical neighborhood used to compute the scatter matrix.
Definition: iss_3d.hpp:50
void getScatterMatrix(const int &current_index, Eigen::Matrix3d &cov_m)
Compute the scatter matrix for a point index.
Definition: iss_3d.hpp:165
void setThreshold21(double gamma_21)
Set the upper bound on the ratio between the second and the first eigenvalue.
Definition: iss_3d.hpp:78
void setMinNeighbors(int min_neighbors)
Set the minimum number of neighbors that has to be found while applying the non maxima suppression al...
Definition: iss_3d.hpp:92
void setNormalRadius(double normal_radius)
Set the radius used for the estimation of the surface normals of the input cloud.
Definition: iss_3d.hpp:64
void setThreshold32(double gamma_32)
Set the upper bound on the ratio between the third and the second eigenvalue.
Definition: iss_3d.hpp:85
bool initCompute() override
Perform the initial checks before computing the keypoints.
Definition: iss_3d.hpp:215
typename PointCloudN::Ptr PointCloudNPtr
Definition: iss_3d.h:95
void setNonMaxRadius(double non_max_radius)
Set the radius for the application of the non maxima supression algorithm.
Definition: iss_3d.hpp:57
bool * getBoundaryPoints(PointCloudIn &input, double border_radius, float angle_threshold)
Compute the boundary points for the given input cloud.
Definition: iss_3d.hpp:120
void detectKeypoints(PointCloudOut &output) override
Detect the keypoints by performing the EVD of the scatter matrix.
Definition: iss_3d.hpp:307
typename Keypoint< PointInT, PointOutT >::PointCloudOut PointCloudOut
Definition: iss_3d.h:92
Surface normal estimation on organized data using integral images.
void setNormalEstimationMethod(NormalEstimationMethod normal_estimation_method)
Set the normal estimation method.
void setInputCloud(const typename PointCloudIn::ConstPtr &cloud) override
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
Keypoint represents the base class for key points.
Definition: keypoint.h:49
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point.
Definition: normal_3d.h:244
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
Definition: normal_3d.h:332
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:65
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...
Definition: point_tests.h:55
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133