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
52 std::size_t npts = input_->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
70 Eigen::Array4f min_array =
71 Eigen::Array4f::Constant(std::numeric_limits<float>::max());
72 Eigen::Array4f max_array =
73 Eigen::Array4f::Constant(std::numeric_limits<float>::lowest());
75 for (
const auto& point : cloud) {
76 min_array = min_array.min(point.getArray4fMap());
77 max_array = max_array.max(point.getArray4fMap());
80 max_vec = max_array.head<3>();
81 min_vec = min_array.head<3>();
85 template<
typename Po
intT>
void
87 const PointCloud& cloud,
const int first,
const int last,
88 const Vector min_values,
const Vector max_values,
91 const int count (last - first);
92 if (count <=
static_cast<int> (sample_))
94 samplePartition (cloud, first, last, indices, output);
98 (max_values - min_values).maxCoeff (&cutDim);
100 const int rightCount (count / 2);
101 const int leftCount (count - rightCount);
102 assert (last - rightCount == first + leftCount);
105 std::nth_element (indices.begin () + first, indices.begin () + first + leftCount,
106 indices.begin () + last, CompareDim (cutDim, cloud));
108 const int cutIndex (indices[first+leftCount]);
109 const float cutVal = findCutVal (cloud, cutDim, cutIndex);
112 Vector leftMaxValues (max_values);
113 leftMaxValues[cutDim] = cutVal;
115 Vector rightMinValues (min_values);
116 rightMinValues[cutDim] = cutVal;
119 partition (cloud, first, first + leftCount, min_values, leftMaxValues, indices, output);
120 partition (cloud, first + leftCount, last, rightMinValues, max_values, indices, output);
124 template<
typename Po
intT>
void
126 const PointCloud& data,
const int first,
const int last,
131 for (
int i = first; i < last; i++)
134 pt.x = data[indices[i]].x;
135 pt.y = data[indices[i]].y;
136 pt.z = data[indices[i]].z;
142 Eigen::Vector4f normal;
146 computeNormal (cloud, normal, curvature);
148 for (
const auto& point: cloud)
151 const float r = float (std::rand ()) / float (RAND_MAX);
156 pt.normal[0] = normal (0);
157 pt.normal[1] = normal (1);
158 pt.normal[2] = normal (2);
159 pt.curvature = curvature;
167 template<
typename Po
intT>
void
170 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
171 Eigen::Vector4f xyz_centroid;
178 nx = ny = nz = curvature = std::numeric_limits<float>::quiet_NaN ();
190 normal (3) = -1 * normal.dot (xyz_centroid);
194 template <
typename Po
intT>
inline unsigned int
196 Eigen::Matrix3f &covariance_matrix,
197 Eigen::Vector4f ¢roid)
200 Eigen::Matrix<float, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<float, 1, 9, Eigen::RowMajor>::Zero ();
201 std::size_t point_count = 0;
202 for (
const auto& point: cloud)
204 if (!isXYZFinite (point))
210 accu [0] += point.x * point.x;
211 accu [1] += point.x * point.y;
212 accu [2] += point.x * point.z;
213 accu [3] += point.y * point.y;
214 accu [4] += point.y * point.z;
215 accu [5] += point.z * point.z;
221 accu /=
static_cast<float> (point_count);
222 centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8];
224 covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
225 covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
226 covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
227 covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
228 covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
229 covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
230 covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
231 covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
232 covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
234 return (
static_cast<unsigned int> (point_count));
238 template <
typename Po
intT>
void
240 float &nx,
float &ny,
float &nz,
float &curvature)
243 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
244 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
245 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
247 nx = eigen_vector [0];
248 ny = eigen_vector [1];
249 nz = eigen_vector [2];
252 float eig_sum = covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8);
254 curvature = std::abs (eigen_value / eig_sum);
260 template <
typename Po
intT>
float
262 const PointCloud& cloud,
const int cut_dim,
const int cut_index)
265 return (cloud[cut_index].x);
267 return (cloud[cut_index].y);
269 return (cloud[cut_index].z);
275 #define PCL_INSTANTIATE_SamplingSurfaceNormal(T) template class PCL_EXPORTS pcl::SamplingSurfaceNormal<T>;
277 #endif // PCL_FILTERS_IMPL_NORMAL_SPACE_SAMPLE_H_