Point Cloud Library (PCL)  1.11.0
sampling_surface_normal.hpp
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_IMPL_SAMPLING_SURFACE_NORMAL_H_
39 #define PCL_FILTERS_IMPL_SAMPLING_SURFACE_NORMAL_H_
40 
41 #include <iostream>
42 #include <vector>
43 #include <pcl/common/eigen.h>
44 #include <pcl/common/point_tests.h> // for pcl::isFinite
45 #include <pcl/filters/sampling_surface_normal.h>
46 
47 ///////////////////////////////////////////////////////////////////////////////
48 template<typename PointT> void
50 {
51  std::vector <int> indices;
52  std::size_t npts = input_->points.size ();
53  for (std::size_t i = 0; i < npts; i++)
54  indices.push_back (i);
55 
56  Vector max_vec (3, 1);
57  Vector min_vec (3, 1);
58  findXYZMaxMin (*input_, max_vec, min_vec);
59  PointCloud data = *input_;
60  partition (data, 0, npts, min_vec, max_vec, indices, output);
61  output.width = 1;
62  output.height = std::uint32_t (output.points.size ());
63 }
64 
65 ///////////////////////////////////////////////////////////////////////////////
66 template<typename PointT> void
67 pcl::SamplingSurfaceNormal<PointT>::findXYZMaxMin (const PointCloud& cloud, Vector& max_vec, Vector& min_vec)
68 {
69  float maxval = cloud.points[0].x;
70  float minval = cloud.points[0].x;
71 
72  for (std::size_t i = 0; i < cloud.points.size (); i++)
73  {
74  if (cloud.points[i].x > maxval)
75  {
76  maxval = cloud.points[i].x;
77  }
78  if (cloud.points[i].x < minval)
79  {
80  minval = cloud.points[i].x;
81  }
82  }
83  max_vec (0) = maxval;
84  min_vec (0) = minval;
85 
86  maxval = cloud.points[0].y;
87  minval = cloud.points[0].y;
88 
89  for (std::size_t i = 0; i < cloud.points.size (); i++)
90  {
91  if (cloud.points[i].y > maxval)
92  {
93  maxval = cloud.points[i].y;
94  }
95  if (cloud.points[i].y < minval)
96  {
97  minval = cloud.points[i].y;
98  }
99  }
100  max_vec (1) = maxval;
101  min_vec (1) = minval;
102 
103  maxval = cloud.points[0].z;
104  minval = cloud.points[0].z;
105 
106  for (std::size_t i = 0; i < cloud.points.size (); i++)
107  {
108  if (cloud.points[i].z > maxval)
109  {
110  maxval = cloud.points[i].z;
111  }
112  if (cloud.points[i].z < minval)
113  {
114  minval = cloud.points[i].z;
115  }
116  }
117  max_vec (2) = maxval;
118  min_vec (2) = minval;
119 }
120 
121 ///////////////////////////////////////////////////////////////////////////////
122 template<typename PointT> void
124  const PointCloud& cloud, const int first, const int last,
125  const Vector min_values, const Vector max_values,
126  std::vector<int>& indices, PointCloud& output)
127 {
128  const int count (last - first);
129  if (count <= static_cast<int> (sample_))
130  {
131  samplePartition (cloud, first, last, indices, output);
132  return;
133  }
134  int cutDim = 0;
135  (max_values - min_values).maxCoeff (&cutDim);
136 
137  const int rightCount (count / 2);
138  const int leftCount (count - rightCount);
139  assert (last - rightCount == first + leftCount);
140 
141  // sort, hack std::nth_element
142  std::nth_element (indices.begin () + first, indices.begin () + first + leftCount,
143  indices.begin () + last, CompareDim (cutDim, cloud));
144 
145  const int cutIndex (indices[first+leftCount]);
146  const float cutVal = findCutVal (cloud, cutDim, cutIndex);
147 
148  // update bounds for left
149  Vector leftMaxValues (max_values);
150  leftMaxValues[cutDim] = cutVal;
151  // update bounds for right
152  Vector rightMinValues (min_values);
153  rightMinValues[cutDim] = cutVal;
154 
155  // recurse
156  partition (cloud, first, first + leftCount, min_values, leftMaxValues, indices, output);
157  partition (cloud, first + leftCount, last, rightMinValues, max_values, indices, output);
158 }
159 
160 ///////////////////////////////////////////////////////////////////////////////
161 template<typename PointT> void
163  const PointCloud& data, const int first, const int last,
164  std::vector <int>& indices, PointCloud& output)
165 {
167 
168  for (int i = first; i < last; i++)
169  {
170  PointT pt;
171  pt.x = data.points[indices[i]].x;
172  pt.y = data.points[indices[i]].y;
173  pt.z = data.points[indices[i]].z;
174  cloud.points.push_back (pt);
175  }
176  cloud.width = 1;
177  cloud.height = std::uint32_t (cloud.points.size ());
178 
179  Eigen::Vector4f normal;
180  float curvature = 0;
181  //pcl::computePointNormal<PointT> (cloud, normal, curvature);
182 
183  computeNormal (cloud, normal, curvature);
184 
185  for (std::size_t i = 0; i < cloud.points.size (); i++)
186  {
187  // TODO: change to Boost random number generators!
188  const float r = float (std::rand ()) / float (RAND_MAX);
189 
190  if (r < ratio_)
191  {
192  PointT pt = cloud.points[i];
193  pt.normal[0] = normal (0);
194  pt.normal[1] = normal (1);
195  pt.normal[2] = normal (2);
196  pt.curvature = curvature;
197 
198  output.points.push_back (pt);
199  }
200  }
201 }
202 
203 ///////////////////////////////////////////////////////////////////////////////
204 template<typename PointT> void
205 pcl::SamplingSurfaceNormal<PointT>::computeNormal (const PointCloud& cloud, Eigen::Vector4f &normal, float& curvature)
206 {
207  EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
208  Eigen::Vector4f xyz_centroid;
209  float nx = 0.0;
210  float ny = 0.0;
211  float nz = 0.0;
212 
213  if (computeMeanAndCovarianceMatrix (cloud, covariance_matrix, xyz_centroid) == 0)
214  {
215  nx = ny = nz = curvature = std::numeric_limits<float>::quiet_NaN ();
216  return;
217  }
218 
219  // Get the plane normal and surface curvature
220  solvePlaneParameters (covariance_matrix, nx, ny, nz, curvature);
221  normal (0) = nx;
222  normal (1) = ny;
223  normal (2) = nz;
224 
225  normal (3) = 0;
226  // Hessian form (D = nc . p_plane (centroid here) + p)
227  normal (3) = -1 * normal.dot (xyz_centroid);
228 }
229 
230 //////////////////////////////////////////////////////////////////////////////////////////////
231 template <typename PointT> inline unsigned int
233  Eigen::Matrix3f &covariance_matrix,
234  Eigen::Vector4f &centroid)
235 {
236  // create the buffer on the stack which is much faster than using cloud.points[indices[i]] and centroid as a buffer
237  Eigen::Matrix<float, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<float, 1, 9, Eigen::RowMajor>::Zero ();
238  std::size_t point_count = 0;
239  for (std::size_t i = 0; i < cloud.points.size (); i++)
240  {
241  if (!isFinite (cloud[i]))
242  {
243  continue;
244  }
245 
246  ++point_count;
247  accu [0] += cloud[i].x * cloud[i].x;
248  accu [1] += cloud[i].x * cloud[i].y;
249  accu [2] += cloud[i].x * cloud[i].z;
250  accu [3] += cloud[i].y * cloud[i].y; // 4
251  accu [4] += cloud[i].y * cloud[i].z; // 5
252  accu [5] += cloud[i].z * cloud[i].z; // 8
253  accu [6] += cloud[i].x;
254  accu [7] += cloud[i].y;
255  accu [8] += cloud[i].z;
256  }
257 
258  accu /= static_cast<float> (point_count);
259  centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8];
260  centroid[3] = 0;
261  covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
262  covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
263  covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
264  covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
265  covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
266  covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
267  covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
268  covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
269  covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
270 
271  return (static_cast<unsigned int> (point_count));
272 }
273 
274 //////////////////////////////////////////////////////////////////////////////////////////////
275 template <typename PointT> void
276 pcl::SamplingSurfaceNormal<PointT>::solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix,
277  float &nx, float &ny, float &nz, float &curvature)
278 {
279  // Extract the smallest eigenvalue and its eigenvector
280  EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
281  EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
282  pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
283 
284  nx = eigen_vector [0];
285  ny = eigen_vector [1];
286  nz = eigen_vector [2];
287 
288  // Compute the curvature surface change
289  float eig_sum = covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8);
290  if (eig_sum != 0)
291  curvature = std::abs (eigen_value / eig_sum);
292  else
293  curvature = 0;
294 }
295 
296 ///////////////////////////////////////////////////////////////////////////////
297 template <typename PointT> float
299  const PointCloud& cloud, const int cut_dim, const int cut_index)
300 {
301  if (cut_dim == 0)
302  return (cloud.points[cut_index].x);
303  if (cut_dim == 1)
304  return (cloud.points[cut_index].y);
305  if (cut_dim == 2)
306  return (cloud.points[cut_index].z);
307 
308  return (0.0f);
309 }
310 
311 
312 #define PCL_INSTANTIATE_SamplingSurfaceNormal(T) template class PCL_EXPORTS pcl::SamplingSurfaceNormal<T>;
313 
314 #endif // PCL_FILTERS_IMPL_NORMAL_SPACE_SAMPLE_H_
pcl::computeMeanAndCovarianceMatrix
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:489
pcl::uint32_t
std::uint32_t uint32_t
Definition: types.h:58
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
pcl::PointCloud::points
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
pcl::solvePlaneParameters
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:51
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: projection_matrix.h:52
pcl::eigen33
void eigen33(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi d...
Definition: eigen.hpp:296
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:628
pcl::SamplingSurfaceNormal::applyFilter
void applyFilter(PointCloud &output) override
Sample of point indices into a separate PointCloud.
Definition: sampling_surface_normal.hpp:49
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
pcl::SamplingSurfaceNormal
SamplingSurfaceNormal divides the input space into grids until each grid contains a maximum of N poin...
Definition: sampling_surface_normal.h:54