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