Point Cloud Library (PCL)  1.7.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 #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_