38 #ifndef PCL_FILTERS_IMPL_SAMPLING_SURFACE_NORMAL_H_
39 #define PCL_FILTERS_IMPL_SAMPLING_SURFACE_NORMAL_H_
43 #include <pcl/common/eigen.h>
44 #include <pcl/common/point_tests.h>
45 #include <pcl/filters/sampling_surface_normal.h>
48 template<
typename Po
intT>
void
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);
56 Vector max_vec (3, 1);
57 Vector min_vec (3, 1);
58 findXYZMaxMin (*input_, max_vec, min_vec);
60 partition (data, 0, npts, min_vec, max_vec, indices, output);
66 template<
typename Po
intT>
void
69 float maxval = cloud.
points[0].x;
70 float minval = cloud.
points[0].x;
72 for (std::size_t i = 0; i < cloud.
points.size (); i++)
74 if (cloud.
points[i].x > maxval)
76 maxval = cloud.
points[i].x;
78 if (cloud.
points[i].x < minval)
80 minval = cloud.
points[i].x;
86 maxval = cloud.
points[0].y;
87 minval = cloud.
points[0].y;
89 for (std::size_t i = 0; i < cloud.
points.size (); i++)
91 if (cloud.
points[i].y > maxval)
93 maxval = cloud.
points[i].y;
95 if (cloud.
points[i].y < minval)
97 minval = cloud.
points[i].y;
100 max_vec (1) = maxval;
101 min_vec (1) = minval;
103 maxval = cloud.
points[0].z;
104 minval = cloud.
points[0].z;
106 for (std::size_t i = 0; i < cloud.
points.size (); i++)
108 if (cloud.
points[i].z > maxval)
110 maxval = cloud.
points[i].z;
112 if (cloud.
points[i].z < minval)
114 minval = cloud.
points[i].z;
117 max_vec (2) = maxval;
118 min_vec (2) = minval;
122 template<
typename Po
intT>
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)
128 const int count (last - first);
129 if (count <= static_cast<int> (sample_))
131 samplePartition (cloud, first, last, indices, output);
135 (max_values - min_values).maxCoeff (&cutDim);
137 const int rightCount (count / 2);
138 const int leftCount (count - rightCount);
139 assert (last - rightCount == first + leftCount);
142 std::nth_element (indices.begin () + first, indices.begin () + first + leftCount,
143 indices.begin () + last, CompareDim (cutDim, cloud));
145 const int cutIndex (indices[first+leftCount]);
146 const float cutVal = findCutVal (cloud, cutDim, cutIndex);
149 Vector leftMaxValues (max_values);
150 leftMaxValues[cutDim] = cutVal;
152 Vector rightMinValues (min_values);
153 rightMinValues[cutDim] = cutVal;
156 partition (cloud, first, first + leftCount, min_values, leftMaxValues, indices, output);
157 partition (cloud, first + leftCount, last, rightMinValues, max_values, indices, output);
161 template<
typename Po
intT>
void
163 const PointCloud& data,
const int first,
const int last,
164 std::vector <int>& indices,
PointCloud& output)
168 for (
int i = first; i < last; i++)
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);
179 Eigen::Vector4f normal;
183 computeNormal (cloud, normal, curvature);
185 for (std::size_t i = 0; i < cloud.
points.size (); i++)
188 const float r = float (std::rand ()) / float (RAND_MAX);
193 pt.normal[0] = normal (0);
194 pt.normal[1] = normal (1);
195 pt.normal[2] = normal (2);
196 pt.curvature = curvature;
198 output.
points.push_back (pt);
204 template<
typename Po
intT>
void
207 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
208 Eigen::Vector4f xyz_centroid;
215 nx = ny = nz = curvature = std::numeric_limits<float>::quiet_NaN ();
227 normal (3) = -1 * normal.dot (xyz_centroid);
231 template <
typename Po
intT>
inline unsigned int
233 Eigen::Matrix3f &covariance_matrix,
234 Eigen::Vector4f ¢roid)
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++)
241 if (!isFinite (cloud[i]))
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;
251 accu [4] += cloud[i].y * cloud[i].z;
252 accu [5] += cloud[i].z * cloud[i].z;
253 accu [6] += cloud[i].x;
254 accu [7] += cloud[i].y;
255 accu [8] += cloud[i].z;
258 accu /=
static_cast<float> (point_count);
259 centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8];
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);
271 return (static_cast<unsigned int> (point_count));
275 template <
typename Po
intT>
void
277 float &nx,
float &ny,
float &nz,
float &curvature)
280 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
281 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
282 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
284 nx = eigen_vector [0];
285 ny = eigen_vector [1];
286 nz = eigen_vector [2];
289 float eig_sum = covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8);
291 curvature = std::abs (eigen_value / eig_sum);
297 template <
typename Po
intT>
float
299 const PointCloud& cloud,
const int cut_dim,
const int cut_index)
302 return (cloud.
points[cut_index].x);
304 return (cloud.
points[cut_index].y);
306 return (cloud.
points[cut_index].z);
312 #define PCL_INSTANTIATE_SamplingSurfaceNormal(T) template class PCL_EXPORTS pcl::SamplingSurfaceNormal<T>;
314 #endif // PCL_FILTERS_IMPL_NORMAL_SPACE_SAMPLE_H_
SamplingSurfaceNormal divides the input space into grids until each grid contains a maximum of N poin...
unsigned int computeMeanAndCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix, Eigen::Matrix< Scalar, 4, 1 > ¢roid)
Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single lo...
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...
std::uint32_t width
The point cloud width (if organized as an image-structure).
void applyFilter(PointCloud &output) override
Sample of point indices into a separate PointCloud.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
std::uint32_t height
The point cloud height (if organized as an image-structure).
A point structure representing Euclidean xyz coordinates, and the RGB color.