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