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