Point Cloud Library (PCL)  1.15.0-dev
feature.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  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id$
38  *
39  */
40 
41 #ifndef PCL_FEATURES_IMPL_FEATURE_H_
42 #define PCL_FEATURES_IMPL_FEATURE_H_
43 
44 #include <pcl/search/kdtree.h> // for KdTree
45 #include <pcl/search/organized.h> // for OrganizedNeighbor
46 
47 
48 namespace pcl
49 {
50 
51 inline void
52 solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix,
53  const Eigen::Vector4f &point,
54  Eigen::Vector4f &plane_parameters, float &curvature)
55 {
56  solvePlaneParameters (covariance_matrix, plane_parameters [0], plane_parameters [1], plane_parameters [2], curvature);
57 
58  plane_parameters[3] = 0;
59  // Hessian form (D = nc . p_plane (centroid here) + p)
60  plane_parameters[3] = -1 * plane_parameters.dot (point);
61 }
62 
63 
64 inline void
65 solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix,
66  float &nx, float &ny, float &nz, float &curvature)
67 {
68  // Avoid getting hung on Eigen's optimizers
69 // for (int i = 0; i < 9; ++i)
70 // if (!std::isfinite (covariance_matrix.coeff (i)))
71 // {
72 // //PCL_WARN ("[pcl::solvePlaneParameters] Covariance matrix has NaN/Inf values!\n");
73 // nx = ny = nz = curvature = std::numeric_limits<float>::quiet_NaN ();
74 // return;
75 // }
76  // Extract the smallest eigenvalue and its eigenvector
77  EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
78  EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
79  pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
80 
81  nx = eigen_vector [0];
82  ny = eigen_vector [1];
83  nz = eigen_vector [2];
84 
85  // Compute the curvature surface change
86  float eig_sum = covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8);
87  if (eig_sum != 0)
88  curvature = std::abs (eigen_value / eig_sum);
89  else
90  curvature = 0;
91 }
92 
93 
94 template <typename PointInT, typename PointOutT> bool
96 {
98  {
99  PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
100  return (false);
101  }
102 
103  // If the dataset is empty, just return
104  if (input_->points.empty ())
105  {
106  PCL_ERROR ("[pcl::%s::compute] input_ is empty!\n", getClassName ().c_str ());
107  // Cleanup
108  deinitCompute ();
109  return (false);
110  }
111 
112  // If no search surface has been defined, use the input dataset as the search surface itself
113  if (!surface_)
114  {
115  fake_surface_ = true;
116  surface_ = input_;
117  }
118 
119  // Check if a space search locator was given
120  if (!tree_)
121  {
122  if (surface_->isOrganized () && input_->isOrganized ()) {
123  tree_.reset (new pcl::search::OrganizedNeighbor<PointInT> ());
124  if(!tree_->setInputCloud (surface_)) { // may return false if OrganizedNeighbor cannot work with the cloud, then use KdTree instead
125  tree_.reset (new pcl::search::KdTree<PointInT> (false));
126  }
127  } else {
128  tree_.reset (new pcl::search::KdTree<PointInT> (false));
129  }
130  }
131 
132  if (tree_->getInputCloud () != surface_) { // Make sure the tree searches the surface
133  if(!tree_->setInputCloud (surface_)) {
134  PCL_ERROR ("[pcl::%s::compute] The given search method cannot work with the given input cloud/search surface.\n", getClassName ().c_str ());
135  return (false);
136  }
137  }
138 
139 
140  // Do a fast check to see if the search parameters are well defined
141  if (search_radius_ != 0.0)
142  {
143  if (k_ != 0)
144  {
145  PCL_ERROR ("[pcl::%s::compute] ", getClassName ().c_str ());
146  PCL_ERROR ("Both radius (%f) and K (%d) defined! ", search_radius_, k_);
147  PCL_ERROR ("Set one of them to zero first and then re-run compute ().\n");
148  // Cleanup
149  deinitCompute ();
150  return (false);
151  }
152  else // Use the radiusSearch () function
153  {
154  search_parameter_ = search_radius_;
155  // Declare the search locator definition
156  search_method_surface_ = [this] (const PointCloudIn &cloud, int index, double radius,
157  pcl::Indices &k_indices, std::vector<float> &k_distances)
158  {
159  return tree_->radiusSearch (cloud, index, radius, k_indices, k_distances, 0);
160  };
161  }
162  }
163  else
164  {
165  if (k_ != 0) // Use the nearestKSearch () function
166  {
167  search_parameter_ = k_;
168  // Declare the search locator definition
169  search_method_surface_ = [this] (const PointCloudIn &cloud, int index, int k, pcl::Indices &k_indices,
170  std::vector<float> &k_distances)
171  {
172  return tree_->nearestKSearch (cloud, index, k, k_indices, k_distances);
173  };
174  }
175  else
176  {
177  PCL_ERROR ("[pcl::%s::compute] Neither radius nor K defined! ", getClassName ().c_str ());
178  PCL_ERROR ("Set one of them to a positive number first and then re-run compute ().\n");
179  // Cleanup
180  deinitCompute ();
181  return (false);
182  }
183  }
184  return (true);
185 }
186 
187 
188 template <typename PointInT, typename PointOutT> bool
190 {
191  // Reset the surface
192  if (fake_surface_)
193  {
194  surface_.reset ();
195  fake_surface_ = false;
196  }
197  return (true);
198 }
199 
200 
201 template <typename PointInT, typename PointOutT> void
203 {
204  if (!initCompute ())
205  {
206  output.width = output.height = 0;
207  output.clear ();
208  return;
209  }
210 
211  // Copy the header
212  output.header = input_->header;
213 
214  // Resize the output dataset
215  if (output.size () != indices_->size ())
216  output.resize (indices_->size ());
217 
218  // Check if the output will be computed for all points or only a subset
219  // If the input width or height are not set, set output width as size
220  if (indices_->size () != input_->points.size () || input_->width * input_->height == 0)
221  {
222  output.width = indices_->size ();
223  output.height = 1;
224  }
225  else
226  {
227  output.width = input_->width;
228  output.height = input_->height;
229  }
230  output.is_dense = input_->is_dense;
231 
232  // Perform the actual feature computation
233  computeFeature (output);
234 
235  deinitCompute ();
236 }
237 
238 
239 template <typename PointInT, typename PointNT, typename PointOutT> bool
241 {
243  {
244  PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
245  return (false);
246  }
247 
248  // Check if input normals are set
249  if (!normals_)
250  {
251  PCL_ERROR ("[pcl::%s::initCompute] No input dataset containing normals was given!\n", getClassName ().c_str ());
253  return (false);
254  }
255 
256  // Check if the size of normals is the same as the size of the surface
257  if (normals_->points.size () != surface_->points.size ())
258  {
259  PCL_ERROR ("[pcl::%s::initCompute] ", getClassName ().c_str ());
260  PCL_ERROR("The number of points in the surface dataset (%zu) differs from ",
261  static_cast<std::size_t>(surface_->points.size()));
262  PCL_ERROR("the number of points in the dataset containing the normals (%zu)!\n",
263  static_cast<std::size_t>(normals_->points.size()));
265  return (false);
266  }
267 
268  return (true);
269 }
270 
271 
272 template <typename PointInT, typename PointLT, typename PointOutT> bool
274 {
276  {
277  PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
278  return (false);
279  }
280 
281  // Check if input normals are set
282  if (!labels_)
283  {
284  PCL_ERROR ("[pcl::%s::initCompute] No input dataset containing labels was given!\n", getClassName ().c_str ());
286  return (false);
287  }
288 
289  // Check if the size of normals is the same as the size of the surface
290  if (labels_->points.size () != surface_->points.size ())
291  {
292  PCL_ERROR ("[pcl::%s::initCompute] The number of points in the input dataset differs from the number of points in the dataset containing the labels!\n", getClassName ().c_str ());
294  return (false);
295  }
296 
297  return (true);
298 }
299 
300 
301 template <typename PointInT, typename PointRFT> bool
303  const LRFEstimationPtr& lrf_estimation)
304 {
305  if (frames_never_defined_)
306  frames_.reset ();
307 
308  // Check if input frames are set
309  if (!frames_)
310  {
311  if (!lrf_estimation)
312  {
313  PCL_ERROR ("[initLocalReferenceFrames] No input dataset containing reference frames was given!\n");
314  return (false);
315  } else
316  {
317  //PCL_WARN ("[initLocalReferenceFrames] No input dataset containing reference frames was given! Proceed using default\n");
318  PointCloudLRFPtr default_frames (new PointCloudLRF());
319  lrf_estimation->compute (*default_frames);
320  frames_ = default_frames;
321  }
322  }
323 
324  // Check if the size of frames is the same as the size of the input cloud
325  if (frames_->points.size () != indices_size)
326  {
327  if (!lrf_estimation)
328  {
329  PCL_ERROR ("[initLocalReferenceFrames] The number of points in the input dataset differs from the number of points in the dataset containing the reference frames!\n");
330  return (false);
331  } else
332  {
333  //PCL_WARN ("[initLocalReferenceFrames] The number of points in the input dataset differs from the number of points in the dataset containing the reference frames! Proceed using default\n");
334  PointCloudLRFPtr default_frames (new PointCloudLRF());
335  lrf_estimation->compute (*default_frames);
336  frames_ = default_frames;
337  }
338  }
339 
340  return (true);
341 }
342 
343 } // namespace pcl
344 
345 #endif //#ifndef PCL_FEATURES_IMPL_FEATURE_H_
346 
virtual bool initCompute()
This method should get called before starting the actual computation.
Definition: feature.hpp:273
virtual bool initCompute()
This method should get called before starting the actual computation.
Definition: feature.hpp:240
Feature represents the base feature class.
Definition: feature.h:107
virtual bool initCompute()
This method should get called before starting the actual computation.
Definition: feature.hpp:95
virtual bool deinitCompute()
This method should get called after ending the actual computation.
Definition: feature.hpp:189
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition: feature.hpp:202
typename PointCloudLRF::Ptr PointCloudLRFPtr
Definition: feature.h:443
typename Feature< PointInT, PointRFT >::Ptr LRFEstimationPtr
Check if frames_ has been correctly initialized and compute it if needed.
Definition: feature.h:484
virtual bool initLocalReferenceFrames(const std::size_t &indices_size, const LRFEstimationPtr &lrf_estimation=LRFEstimationPtr())
Definition: feature.hpp:302
PCL base class.
Definition: pcl_base.h:70
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Definition: point_cloud.h:404
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:463
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:399
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:393
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:401
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:886
std::size_t size() const
Definition: point_cloud.h:444
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:62
OrganizedNeighbor is a class for optimized nearest neighbor search in organized projectable point clo...
Definition: organized.h:66
void eigen33(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi d...
Definition: eigen.hpp:295
void solvePlaneParameters(const Eigen::Matrix3f &covariance_matrix, const Eigen::Vector4f &point, Eigen::Vector4f &plane_parameters, float &curvature)
Solve the eigenvalues and eigenvectors of a given 3x3 covariance matrix, and estimate the least-squar...
Definition: feature.hpp:52
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133