Point Cloud Library (PCL)  1.11.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/pcl_search.h>
45 
46 
47 namespace pcl
48 {
49 
50 inline void
51 solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix,
52  const Eigen::Vector4f &point,
53  Eigen::Vector4f &plane_parameters, float &curvature)
54 {
55  solvePlaneParameters (covariance_matrix, plane_parameters [0], plane_parameters [1], plane_parameters [2], curvature);
56 
57  plane_parameters[3] = 0;
58  // Hessian form (D = nc . p_plane (centroid here) + p)
59  plane_parameters[3] = -1 * plane_parameters.dot (point);
60 }
61 
62 
63 inline void
64 solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix,
65  float &nx, float &ny, float &nz, float &curvature)
66 {
67  // Avoid getting hung on Eigen's optimizers
68 // for (int i = 0; i < 9; ++i)
69 // if (!std::isfinite (covariance_matrix.coeff (i)))
70 // {
71 // //PCL_WARN ("[pcl::solvePlaneParameteres] Covariance matrix has NaN/Inf values!\n");
72 // nx = ny = nz = curvature = std::numeric_limits<float>::quiet_NaN ();
73 // return;
74 // }
75  // Extract the smallest eigenvalue and its eigenvector
76  EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
77  EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
78  pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
79 
80  nx = eigen_vector [0];
81  ny = eigen_vector [1];
82  nz = eigen_vector [2];
83 
84  // Compute the curvature surface change
85  float eig_sum = covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8);
86  if (eig_sum != 0)
87  curvature = std::abs (eigen_value / eig_sum);
88  else
89  curvature = 0;
90 }
91 
92 
93 template <typename PointInT, typename PointOutT> bool
95 {
97  {
98  PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
99  return (false);
100  }
101 
102  // If the dataset is empty, just return
103  if (input_->points.empty ())
104  {
105  PCL_ERROR ("[pcl::%s::compute] input_ is empty!\n", getClassName ().c_str ());
106  // Cleanup
107  deinitCompute ();
108  return (false);
109  }
110 
111  // If no search surface has been defined, use the input dataset as the search surface itself
112  if (!surface_)
113  {
114  fake_surface_ = true;
115  surface_ = input_;
116  }
117 
118  // Check if a space search locator was given
119  if (!tree_)
120  {
121  if (surface_->isOrganized () && input_->isOrganized ())
122  tree_.reset (new pcl::search::OrganizedNeighbor<PointInT> ());
123  else
124  tree_.reset (new pcl::search::KdTree<PointInT> (false));
125  }
126 
127  if (tree_->getInputCloud () != surface_) // Make sure the tree searches the surface
128  tree_->setInputCloud (surface_);
129 
130 
131  // Do a fast check to see if the search parameters are well defined
132  if (search_radius_ != 0.0)
133  {
134  if (k_ != 0)
135  {
136  PCL_ERROR ("[pcl::%s::compute] ", getClassName ().c_str ());
137  PCL_ERROR ("Both radius (%f) and K (%d) defined! ", search_radius_, k_);
138  PCL_ERROR ("Set one of them to zero first and then re-run compute ().\n");
139  // Cleanup
140  deinitCompute ();
141  return (false);
142  }
143  else // Use the radiusSearch () function
144  {
145  search_parameter_ = search_radius_;
146  // Declare the search locator definition
147  search_method_surface_ = [this] (const PointCloudIn &cloud, int index, double radius,
148  std::vector<int> &k_indices, std::vector<float> &k_distances)
149  {
150  return tree_->radiusSearch (cloud, index, radius, k_indices, k_distances, 0);
151  };
152  }
153  }
154  else
155  {
156  if (k_ != 0) // Use the nearestKSearch () function
157  {
158  search_parameter_ = k_;
159  // Declare the search locator definition
160  search_method_surface_ = [this] (const PointCloudIn &cloud, int index, int k, std::vector<int> &k_indices,
161  std::vector<float> &k_distances)
162  {
163  return tree_->nearestKSearch (cloud, index, k, k_indices, k_distances);
164  };
165  }
166  else
167  {
168  PCL_ERROR ("[pcl::%s::compute] Neither radius nor K defined! ", getClassName ().c_str ());
169  PCL_ERROR ("Set one of them to a positive number first and then re-run compute ().\n");
170  // Cleanup
171  deinitCompute ();
172  return (false);
173  }
174  }
175  return (true);
176 }
177 
178 
179 template <typename PointInT, typename PointOutT> bool
181 {
182  // Reset the surface
183  if (fake_surface_)
184  {
185  surface_.reset ();
186  fake_surface_ = false;
187  }
188  return (true);
189 }
190 
191 
192 template <typename PointInT, typename PointOutT> void
194 {
195  if (!initCompute ())
196  {
197  output.width = output.height = 0;
198  output.points.clear ();
199  return;
200  }
201 
202  // Copy the header
203  output.header = input_->header;
204 
205  // Resize the output dataset
206  if (output.points.size () != indices_->size ())
207  output.points.resize (indices_->size ());
208 
209  // Check if the output will be computed for all points or only a subset
210  // If the input width or height are not set, set output width as size
211  if (indices_->size () != input_->points.size () || input_->width * input_->height == 0)
212  {
213  output.width = indices_->size ();
214  output.height = 1;
215  }
216  else
217  {
218  output.width = input_->width;
219  output.height = input_->height;
220  }
221  output.is_dense = input_->is_dense;
222 
223  // Perform the actual feature computation
224  computeFeature (output);
225 
226  deinitCompute ();
227 }
228 
229 
230 template <typename PointInT, typename PointNT, typename PointOutT> bool
232 {
234  {
235  PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
236  return (false);
237  }
238 
239  // Check if input normals are set
240  if (!normals_)
241  {
242  PCL_ERROR ("[pcl::%s::initCompute] No input dataset containing normals was given!\n", getClassName ().c_str ());
244  return (false);
245  }
246 
247  // Check if the size of normals is the same as the size of the surface
248  if (normals_->points.size () != surface_->points.size ())
249  {
250  PCL_ERROR ("[pcl::%s::initCompute] ", getClassName ().c_str ());
251  PCL_ERROR("The number of points in the input dataset (%zu) differs from ",
252  static_cast<std::size_t>(surface_->points.size()));
253  PCL_ERROR("the number of points in the dataset containing the normals (%zu)!\n",
254  static_cast<std::size_t>(normals_->points.size()));
256  return (false);
257  }
258 
259  return (true);
260 }
261 
262 
263 template <typename PointInT, typename PointLT, typename PointOutT> bool
265 {
267  {
268  PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
269  return (false);
270  }
271 
272  // Check if input normals are set
273  if (!labels_)
274  {
275  PCL_ERROR ("[pcl::%s::initCompute] No input dataset containing labels was given!\n", getClassName ().c_str ());
277  return (false);
278  }
279 
280  // Check if the size of normals is the same as the size of the surface
281  if (labels_->points.size () != surface_->points.size ())
282  {
283  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 ());
285  return (false);
286  }
287 
288  return (true);
289 }
290 
291 
292 template <typename PointInT, typename PointRFT> bool
294  const LRFEstimationPtr& lrf_estimation)
295 {
296  if (frames_never_defined_)
297  frames_.reset ();
298 
299  // Check if input frames are set
300  if (!frames_)
301  {
302  if (!lrf_estimation)
303  {
304  PCL_ERROR ("[initLocalReferenceFrames] No input dataset containing reference frames was given!\n");
305  return (false);
306  } else
307  {
308  //PCL_WARN ("[initLocalReferenceFrames] No input dataset containing reference frames was given! Proceed using default\n");
309  PointCloudLRFPtr default_frames (new PointCloudLRF());
310  lrf_estimation->compute (*default_frames);
311  frames_ = default_frames;
312  }
313  }
314 
315  // Check if the size of frames is the same as the size of the input cloud
316  if (frames_->points.size () != indices_size)
317  {
318  if (!lrf_estimation)
319  {
320  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");
321  return (false);
322  } else
323  {
324  //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");
325  PointCloudLRFPtr default_frames (new PointCloudLRF());
326  lrf_estimation->compute (*default_frames);
327  frames_ = default_frames;
328  }
329  }
330 
331  return (true);
332 }
333 
334 } // namespace pcl
335 
336 #endif //#ifndef PCL_FEATURES_IMPL_FEATURE_H_
337 
pcl
Definition: convolution.h:46
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:416
pcl::PointCloud::points
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:411
pcl::Feature::deinitCompute
virtual bool deinitCompute()
This method should get called after ending the actual computation.
Definition: feature.hpp:180
pcl::solvePlaneParameters
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:51
pcl::PCLBase
PCL base class.
Definition: pcl_base.h:72
pcl::PointCloud< PointInT >
pcl::eigen33
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:296
pcl::FeatureFromNormals::initCompute
virtual bool initCompute()
This method should get called before starting the actual computation.
Definition: feature.hpp:231
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:414
pcl::Feature::initCompute
virtual bool initCompute()
This method should get called before starting the actual computation.
Definition: feature.hpp:94
pcl::search::KdTree
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:61
pcl::Feature::compute
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition: feature.hpp:193
pcl::FeatureWithLocalReferenceFrames< PointInT, pcl::ReferenceFrame >::LRFEstimationPtr
typename Feature< PointInT, pcl::ReferenceFrame >::Ptr LRFEstimationPtr
Check if frames_ has been correctly initialized and compute it if needed.
Definition: feature.h:492
pcl::PointCloud::is_dense
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:419
pcl::PointCloud::header
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:408
pcl::FeatureWithLocalReferenceFrames::initLocalReferenceFrames
virtual bool initLocalReferenceFrames(const std::size_t &indices_size, const LRFEstimationPtr &lrf_estimation=LRFEstimationPtr())
Definition: feature.hpp:293
pcl::search::OrganizedNeighbor
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds.
Definition: organized.h:63
pcl::FeatureWithLocalReferenceFrames< PointInT, pcl::ReferenceFrame >::PointCloudLRFPtr
typename PointCloudLRF::Ptr PointCloudLRFPtr
Definition: feature.h:452
pcl::FeatureFromLabels::initCompute
virtual bool initCompute()
This method should get called before starting the actual computation.
Definition: feature.hpp:264
pcl::Feature
Feature represents the base feature class.
Definition: feature.h:106