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