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