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