Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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
48namespace pcl
49{
50
51inline void
52solvePlaneParameters (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
64inline void
65solvePlaneParameters (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
94template <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 else
125 tree_.reset (new pcl::search::KdTree<PointInT> (false));
126 }
127
128 if (tree_->getInputCloud () != surface_) // Make sure the tree searches the surface
129 tree_->setInputCloud (surface_);
130
131
132 // Do a fast check to see if the search parameters are well defined
133 if (search_radius_ != 0.0)
134 {
135 if (k_ != 0)
136 {
137 PCL_ERROR ("[pcl::%s::compute] ", getClassName ().c_str ());
138 PCL_ERROR ("Both radius (%f) and K (%d) defined! ", search_radius_, k_);
139 PCL_ERROR ("Set one of them to zero first and then re-run compute ().\n");
140 // Cleanup
141 deinitCompute ();
142 return (false);
143 }
144 else // Use the radiusSearch () function
145 {
146 search_parameter_ = search_radius_;
147 // Declare the search locator definition
148 search_method_surface_ = [this] (const PointCloudIn &cloud, int index, double radius,
149 pcl::Indices &k_indices, std::vector<float> &k_distances)
150 {
151 return tree_->radiusSearch (cloud, index, radius, k_indices, k_distances, 0);
152 };
153 }
154 }
155 else
156 {
157 if (k_ != 0) // Use the nearestKSearch () function
158 {
159 search_parameter_ = k_;
160 // Declare the search locator definition
161 search_method_surface_ = [this] (const PointCloudIn &cloud, int index, int k, pcl::Indices &k_indices,
162 std::vector<float> &k_distances)
163 {
164 return tree_->nearestKSearch (cloud, index, k, k_indices, k_distances);
165 };
166 }
167 else
168 {
169 PCL_ERROR ("[pcl::%s::compute] Neither radius nor K defined! ", getClassName ().c_str ());
170 PCL_ERROR ("Set one of them to a positive number first and then re-run compute ().\n");
171 // Cleanup
172 deinitCompute ();
173 return (false);
174 }
175 }
176 return (true);
177}
178
179
180template <typename PointInT, typename PointOutT> bool
182{
183 // Reset the surface
184 if (fake_surface_)
185 {
186 surface_.reset ();
187 fake_surface_ = false;
188 }
189 return (true);
190}
191
192
193template <typename PointInT, typename PointOutT> void
195{
196 if (!initCompute ())
197 {
198 output.width = output.height = 0;
199 output.clear ();
200 return;
201 }
202
203 // Copy the header
204 output.header = input_->header;
205
206 // Resize the output dataset
207 if (output.size () != indices_->size ())
208 output.resize (indices_->size ());
209
210 // Check if the output will be computed for all points or only a subset
211 // If the input width or height are not set, set output width as size
212 if (indices_->size () != input_->points.size () || input_->width * input_->height == 0)
213 {
214 output.width = indices_->size ();
215 output.height = 1;
217 else
218 {
219 output.width = input_->width;
220 output.height = input_->height;
221 }
222 output.is_dense = input_->is_dense;
223
224 // Perform the actual feature computation
225 computeFeature (output);
226
227 deinitCompute ();
228}
229
230
231template <typename PointInT, typename PointNT, typename PointOutT> bool
233{
235 {
236 PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
237 return (false);
238 }
239
240 // Check if input normals are set
241 if (!normals_)
242 {
243 PCL_ERROR ("[pcl::%s::initCompute] No input dataset containing normals was given!\n", getClassName ().c_str ());
245 return (false);
246 }
247
248 // Check if the size of normals is the same as the size of the surface
249 if (normals_->points.size () != surface_->points.size ())
250 {
251 PCL_ERROR ("[pcl::%s::initCompute] ", getClassName ().c_str ());
252 PCL_ERROR("The number of points in the surface dataset (%zu) differs from ",
253 static_cast<std::size_t>(surface_->points.size()));
254 PCL_ERROR("the number of points in the dataset containing the normals (%zu)!\n",
255 static_cast<std::size_t>(normals_->points.size()));
257 return (false);
258 }
259
260 return (true);
261}
262
263
264template <typename PointInT, typename PointLT, typename PointOutT> bool
266{
268 {
269 PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
270 return (false);
271 }
272
273 // Check if input normals are set
274 if (!labels_)
275 {
276 PCL_ERROR ("[pcl::%s::initCompute] No input dataset containing labels was given!\n", getClassName ().c_str ());
278 return (false);
279 }
280
281 // Check if the size of normals is the same as the size of the surface
282 if (labels_->points.size () != surface_->points.size ())
283 {
284 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 ());
286 return (false);
287 }
288
289 return (true);
290}
291
292
293template <typename PointInT, typename PointRFT> bool
295 const LRFEstimationPtr& lrf_estimation)
296{
297 if (frames_never_defined_)
298 frames_.reset ();
299
300 // Check if input frames are set
301 if (!frames_)
302 {
303 if (!lrf_estimation)
304 {
305 PCL_ERROR ("[initLocalReferenceFrames] No input dataset containing reference frames was given!\n");
306 return (false);
307 } else
308 {
309 //PCL_WARN ("[initLocalReferenceFrames] No input dataset containing reference frames was given! Proceed using default\n");
310 PointCloudLRFPtr default_frames (new PointCloudLRF());
311 lrf_estimation->compute (*default_frames);
312 frames_ = default_frames;
313 }
314 }
315
316 // Check if the size of frames is the same as the size of the input cloud
317 if (frames_->points.size () != indices_size)
318 {
319 if (!lrf_estimation)
320 {
321 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");
322 return (false);
323 } else
324 {
325 //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");
326 PointCloudLRFPtr default_frames (new PointCloudLRF());
327 lrf_estimation->compute (*default_frames);
328 frames_ = default_frames;
329 }
330 }
331
332 return (true);
333}
334
335} // namespace pcl
336
337#endif //#ifndef PCL_FEATURES_IMPL_FEATURE_H_
338
virtual bool initCompute()
This method should get called before starting the actual computation.
Definition feature.hpp:265
virtual bool initCompute()
This method should get called before starting the actual computation.
Definition feature.hpp:232
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:181
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition feature.hpp:194
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:294
pcl::PointCloud< PointRFT > PointCloudLRF
Definition feature.h:442
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).
void resize(std::size_t count)
Resizes the container to contain count elements.
std::uint32_t width
The point cloud width (if organized as an image-structure).
pcl::PCLHeader header
The point cloud header.
std::uint32_t height
The point cloud height (if organized as an image-structure).
void clear()
Removes all points in a cloud and sets the width and height to 0.
std::size_t size() const
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