Point Cloud Library (PCL)  1.9.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 #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 = boost::shared_ptr<SamplingSurfaceNormal<PointT> >;
70  using ConstPtr = boost::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.points[p0].x < cloud.points[p1].x);
166  if (dim == 1)
167  return (cloud.points[p0].y < cloud.points[p1].y);
168  if (dim == 2)
169  return (cloud.points[p0].z < cloud.points[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
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
SamplingSurfaceNormal divides the input space into grids until each grid contains a maximum of N poin...
boost::shared_ptr< const Filter< PointT > > ConstPtr
Definition: filter.h:90
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:426
unsigned int computeMeanAndCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix, Eigen::Matrix< Scalar, 4, 1 > &centroid)
Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single lo...
Definition: centroid.hpp:483
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
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:48
void applyFilter(PointCloud &output) override
Sample of point indices into a separate PointCloud.
void setSample(unsigned int sample)
Set maximum number of samples in each grid.
Filter represents the base filter class.
Definition: filter.h:83
unsigned int sample_
Maximum number of samples in each grid.
float ratio_
Ratio of points to be sampled in each grid.
typename PointCloud::Ptr PointCloudPtr
Definition: pcl_base.h:73
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:444
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void setRatio(float ratio)
Set ratio of points to be sampled in each grid.
void setSeed(unsigned int seed)
Set seed of random function.
boost::shared_ptr< Filter< PointT > > Ptr
Definition: filter.h:89
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:445
unsigned int getSeed() const
Get the value of the internal seed parameter.
unsigned int getSample() const
Get the value of the internal sample parameter.
std::string filter_name_
The filter name.
Definition: filter.h:164
SamplingSurfaceNormal()
Empty constructor.
unsigned int seed_
Random number seed.
float getRatio() const
Get the value of the internal ratio parameter.
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: pcl_base.h:74