Point Cloud Library (PCL)  1.14.1-dev
sampling_surface_normal.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2009-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 the copyright holder(s) 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 #pragma once
39 
40 #include <pcl/filters/filter.h>
41 #include <ctime>
42 
43 namespace pcl
44 {
45  /** \brief @b SamplingSurfaceNormal divides the input space into grids until each grid contains a maximum of N points,
46  * and samples points randomly within each grid. Normal is computed using the N points of each grid. All points
47  * sampled within a grid are assigned the same normal.
48  *
49  * \author Aravindhan K Krishnan. This code is ported from libpointmatcher (https://github.com/ethz-asl/libpointmatcher)
50  * \ingroup filters
51  */
52  template<typename PointT>
53  class SamplingSurfaceNormal: public Filter<PointT>
54  {
59 
60  using PointCloud = typename Filter<PointT>::PointCloud;
61  using PointCloudPtr = typename PointCloud::Ptr;
63 
64  using Vector = Eigen::Matrix<float, 3, 1>;
65 
66  public:
67 
68  using Ptr = shared_ptr<SamplingSurfaceNormal<PointT> >;
69  using ConstPtr = shared_ptr<const SamplingSurfaceNormal<PointT> >;
70 
71  /** \brief Empty constructor. */
73  {
74  filter_name_ = "SamplingSurfaceNormal";
75  srand (seed_);
76  }
77 
78  /** \brief Set maximum number of samples in each grid
79  * \param[in] sample maximum number of samples in each grid
80  */
81  inline void
82  setSample (unsigned int sample)
83  {
84  sample_ = sample;
85  }
86 
87  /** \brief Get the value of the internal \a sample parameter. */
88  inline unsigned int
89  getSample () const
90  {
91  return (sample_);
92  }
93 
94  /** \brief Set seed of random function.
95  * \param[in] seed the input seed
96  */
97  inline void
98  setSeed (unsigned int seed)
99  {
100  seed_ = seed;
101  srand (seed_);
102  }
103 
104  /** \brief Get the value of the internal \a seed parameter. */
105  inline unsigned int
106  getSeed () const
107  {
108  return (seed_);
109  }
110 
111  /** \brief Set ratio of points to be sampled in each grid
112  * \param[in] ratio sample the ratio of points to be sampled in each grid
113  */
114  inline void
115  setRatio (float ratio)
116  {
117  ratio_ = ratio;
118  }
119 
120  /** \brief Get the value of the internal \a ratio parameter. */
121  inline float
122  getRatio () const
123  {
124  return ratio_;
125  }
126 
127  protected:
128 
129  /** \brief Maximum number of samples in each grid. */
130  unsigned int sample_{10};
131  /** \brief Random number seed. */
132  unsigned int seed_{static_cast<unsigned int> (time (nullptr))};
133  /** \brief Ratio of points to be sampled in each grid */
134  float ratio_{0.0f};
135 
136  /** \brief Sample of point indices into a separate PointCloud
137  * \param[out] output the resultant point cloud
138  */
139  void
140  applyFilter (PointCloud &output) override;
141 
142  private:
143 
144  /** \brief @b CompareDim is a comparator object for sorting across a specific dimension (i,.e X, Y or Z)
145  */
146  struct CompareDim
147  {
148  /** \brief The dimension to sort */
149  const int dim;
150  /** \brief The input point cloud to sort */
151  const pcl::PointCloud <PointT>& cloud;
152 
153  /** \brief Constructor. */
154  CompareDim (const int dim, const pcl::PointCloud <PointT>& cloud) : dim (dim), cloud (cloud)
155  {
156  }
157 
158  /** \brief The operator function for sorting. */
159  bool
160  operator () (const int& p0, const int& p1)
161  {
162  if (dim == 0)
163  return (cloud[p0].x < cloud[p1].x);
164  if (dim == 1)
165  return (cloud[p0].y < cloud[p1].y);
166  if (dim == 2)
167  return (cloud[p0].z < cloud[p1].z);
168  return (false);
169  }
170  };
171 
172  /** \brief Finds the max and min values in each dimension
173  * \param[in] cloud the input cloud
174  * \param[out] max_vec the max value vector
175  * \param[out] min_vec the min value vector
176  */
177  void
178  findXYZMaxMin (const PointCloud& cloud, Vector& max_vec, Vector& min_vec);
179 
180  /** \brief Recursively partition the point cloud, stopping when each grid contains less than sample_ points
181  * Points are randomly sampled when a grid is found
182  * \param cloud
183  * \param first
184  * \param last
185  * \param min_values
186  * \param max_values
187  * \param indices
188  * \param[out] outcloud output the resultant point cloud
189  */
190  void
191  partition (const PointCloud& cloud, const int first, const int last,
192  const Vector min_values, const Vector max_values,
193  Indices& indices, PointCloud& outcloud);
194 
195  /** \brief Randomly sample the points in each grid.
196  * \param[in] data
197  * \param[in] first
198  * \param[in] last
199  * \param[out] indices
200  * \param[out] output the resultant point cloud
201  */
202  void
203  samplePartition (const PointCloud& data, const int first, const int last,
204  Indices& indices, PointCloud& outcloud);
205 
206  /** \brief Returns the threshold for splitting in a given dimension.
207  * \param[in] cloud the input cloud
208  * \param[in] cut_dim the input dimension (0=x, 1=y, 2=z)
209  * \param[in] cut_index the input index in the cloud
210  */
211  float
212  findCutVal (const PointCloud& cloud, const int cut_dim, const int cut_index);
213 
214  /** \brief Computes the normal for points in a grid. This is a port from features to avoid features dependency for
215  * filters
216  * \param[in] cloud The input cloud
217  * \param[out] normal the computed normal
218  * \param[out] curvature the computed curvature
219  */
220  void
221  computeNormal (const PointCloud& cloud, Eigen::Vector4f &normal, float& curvature);
222 
223  /** \brief Computes the covariance matrix for points in the cloud. This is a port from features to avoid features dependency for
224  * filters
225  * \param[in] cloud The input cloud
226  * \param[out] covariance_matrix the covariance matrix
227  * \param[out] centroid the centroid
228  */
229  unsigned int
230  computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
231  Eigen::Matrix3f &covariance_matrix,
232  Eigen::Vector4f &centroid);
233 
234  /** \brief Solve the eigenvalues and eigenvectors of a given 3x3 covariance matrix, and estimate the least-squares
235  * plane normal and surface curvature.
236  * \param[in] covariance_matrix the 3x3 covariance matrix
237  * \param[out] (nx ny nz) plane_parameters the resultant plane parameters as: a, b, c, d (ax + by + cz + d = 0)
238  * \param[out] curvature the estimated surface curvature as a measure of
239  */
240  void
241  solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix,
242  float &nx, float &ny, float &nz, float &curvature);
243  };
244 }
245 
246 #ifdef PCL_NO_PRECOMPILE
247 #include <pcl/filters/impl/sampling_surface_normal.hpp>
248 #endif
Filter represents the base filter class.
Definition: filter.h:81
shared_ptr< Filter< PointT > > Ptr
Definition: filter.h:83
shared_ptr< const Filter< PointT > > ConstPtr
Definition: filter.h:84
std::string filter_name_
The filter name.
Definition: filter.h:158
typename PointCloud::Ptr PointCloudPtr
Definition: pcl_base.h:73
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: pcl_base.h:74
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
SamplingSurfaceNormal divides the input space into grids until each grid contains a maximum of N poin...
void setSample(unsigned int sample)
Set maximum number of samples in each grid.
void setSeed(unsigned int seed)
Set seed of random function.
unsigned int getSample() const
Get the value of the internal sample parameter.
void setRatio(float ratio)
Set ratio of points to be sampled in each grid.
float getRatio() const
Get the value of the internal ratio parameter.
void applyFilter(PointCloud &output) override
Sample of point indices into a separate PointCloud.
SamplingSurfaceNormal()
Empty constructor.
float ratio_
Ratio of points to be sampled in each grid.
unsigned int getSeed() const
Get the value of the internal seed parameter.
unsigned int sample_
Maximum number of samples in each grid.
unsigned int seed_
Random number seed.
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133