40#ifndef PCL_ROPS_ESTIMATION_HPP_
41#define PCL_ROPS_ESTIMATION_HPP_
43#include <pcl/features/rops_estimation.h>
47#include <Eigen/Eigenvalues>
50template <
typename Po
intInT,
typename Po
intOutT>
54 triangles_of_the_point_ (0)
59template <
typename Po
intInT,
typename Po
intOutT>
60pcl::ROPSEstimation <PointInT, PointOutT>::~ROPSEstimation ()
63 triangles_of_the_point_.clear ();
67template <
typename Po
intInT,
typename Po
intOutT>
void
70 if (number_of_bins != 0)
71 number_of_bins_ = number_of_bins;
75template <
typename Po
intInT,
typename Po
intOutT>
unsigned int
78 return (number_of_bins_);
82template <
typename Po
intInT,
typename Po
intOutT>
void
85 if (number_of_rotations != 0)
87 number_of_rotations_ = number_of_rotations;
88 step_ = 90.0f /
static_cast <float> (number_of_rotations_ + 1);
93template <
typename Po
intInT,
typename Po
intOutT>
unsigned int
96 return (number_of_rotations_);
100template <
typename Po
intInT,
typename Po
intOutT>
void
103 if (support_radius > 0.0f)
105 support_radius_ = support_radius;
106 sqr_support_radius_ = support_radius * support_radius;
111template <
typename Po
intInT,
typename Po
intOutT>
float
114 return (support_radius_);
118template <
typename Po
intInT,
typename Po
intOutT>
void
121 triangles_ = triangles;
125template <
typename Po
intInT,
typename Po
intOutT>
void
128 triangles = triangles_;
132template <
typename Po
intInT,
typename Po
intOutT>
void
133pcl::ROPSEstimation <PointInT, PointOutT>::computeFeature (PointCloudOut &output)
135 if (triangles_.empty ())
141 buildListOfPointsTriangles ();
144 unsigned int feature_size = number_of_rotations_ * 3 * 3 * 5;
145 const auto number_of_points = indices_->size ();
147 output.reserve (number_of_points);
149 for (
const auto& idx: *indices_)
151 std::set <unsigned int> local_triangles;
153 getLocalSurface ((*input_)[idx], local_triangles, local_points);
155 Eigen::Matrix3f lrf_matrix;
156 computeLRF ((*input_)[idx], local_triangles, lrf_matrix);
158 PointCloudIn transformed_cloud;
159 transformCloud ((*input_)[idx], lrf_matrix, local_points, transformed_cloud);
161 std::array<PointInT, 3> axes;
162 axes[0].x = 1.0f; axes[0].y = 0.0f; axes[0].z = 0.0f;
163 axes[1].x = 0.0f; axes[1].y = 1.0f; axes[1].z = 0.0f;
164 axes[2].x = 0.0f; axes[2].y = 0.0f; axes[2].z = 1.0f;
165 std::vector <float> feature;
166 for (
const auto &axis : axes)
172 PointCloudIn rotated_cloud;
173 Eigen::Vector3f min, max;
174 rotateCloud (axis, theta, transformed_cloud, rotated_cloud, min, max);
177 for (
unsigned int i_proj = 0; i_proj < 3; i_proj++)
179 Eigen::MatrixXf distribution_matrix;
180 distribution_matrix.resize (number_of_bins_, number_of_bins_);
181 getDistributionMatrix (i_proj, min, max, rotated_cloud, distribution_matrix);
184 std::vector <float> moments;
185 computeCentralMoments (distribution_matrix, moments);
187 feature.insert (feature.end (), moments.begin (), moments.end ());
191 }
while (theta < 90.0f);
194 const float norm = std::accumulate(
195 feature.cbegin(), feature.cend(), 0.f, [](
const auto& sum,
const auto& val) {
196 return sum + std::abs(val);
199 if (norm < std::numeric_limits <float>::epsilon ())
202 invert_norm = 1.0f /
norm;
204 output.emplace_back ();
205 for (std::size_t i_dim = 0; i_dim < feature_size; i_dim++)
206 output.back().histogram[i_dim] = feature[i_dim] * invert_norm;
211template <
typename Po
intInT,
typename Po
intOutT>
void
212pcl::ROPSEstimation <PointInT, PointOutT>::buildListOfPointsTriangles ()
214 triangles_of_the_point_.clear ();
216 std::vector <unsigned int> dummy;
218 triangles_of_the_point_.resize (surface_->points. size (), dummy);
220 for (std::size_t i_triangle = 0; i_triangle < triangles_.size (); i_triangle++)
221 for (
const auto& vertex: triangles_[i_triangle].vertices)
222 triangles_of_the_point_[vertex].push_back (i_triangle);
226template <
typename Po
intInT,
typename Po
intOutT>
void
227pcl::ROPSEstimation <PointInT, PointOutT>::getLocalSurface (
const PointInT& point, std::set <unsigned int>& local_triangles,
pcl::Indices& local_points)
const
229 std::vector <float> distances;
230 tree_->radiusSearch (point, support_radius_, local_points, distances);
232 for (
const auto& pt: local_points)
233 local_triangles.insert (triangles_of_the_point_[pt].begin (),
234 triangles_of_the_point_[pt].end ());
238template <
typename Po
intInT,
typename Po
intOutT>
void
239pcl::ROPSEstimation <PointInT, PointOutT>::computeLRF (
const PointInT& point,
const std::set <unsigned int>& local_triangles, Eigen::Matrix3f& lrf_matrix)
const
241 std::size_t number_of_triangles = local_triangles.size ();
243 std::vector<Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > scatter_matrices;
244 std::vector <float> triangle_area (number_of_triangles), distance_weight (number_of_triangles);
246 scatter_matrices.reserve (number_of_triangles);
247 triangle_area.clear ();
248 distance_weight.clear ();
250 float total_area = 0.0f;
251 const float coeff = 1.0f / 12.0f;
252 const float coeff_1_div_3 = 1.0f / 3.0f;
254 Eigen::Vector3f feature_point (point.x, point.y, point.z);
256 for (
const auto& triangle: local_triangles)
258 Eigen::Vector3f pt[3];
259 for (
unsigned int i_vertex = 0; i_vertex < 3; i_vertex++)
261 const unsigned int index = triangles_[triangle].vertices[i_vertex];
262 pt[i_vertex] (0) = (*surface_)[index].x;
263 pt[i_vertex] (1) = (*surface_)[index].y;
264 pt[i_vertex] (2) = (*surface_)[index].z;
267 const float curr_area = ((pt[1] - pt[0]).cross (pt[2] - pt[0])).norm ();
268 triangle_area.push_back (curr_area);
269 total_area += curr_area;
271 distance_weight.push_back (std::pow (support_radius_ - (feature_point - (pt[0] + pt[1] + pt[2]) * coeff_1_div_3).norm (), 2.0f));
273 Eigen::Matrix3f curr_scatter_matrix;
274 curr_scatter_matrix.setZero ();
275 for (
const auto &i_pt : pt)
277 Eigen::Vector3f vec = i_pt - feature_point;
278 curr_scatter_matrix += vec * (vec.transpose ());
279 for (
const auto &j_pt : pt)
280 curr_scatter_matrix += vec * ((j_pt - feature_point).transpose ());
282 scatter_matrices.emplace_back (coeff * curr_scatter_matrix);
285 if (std::abs (total_area) < std::numeric_limits <float>::epsilon ())
288 total_area = 1.0f / total_area;
290 Eigen::Matrix3f overall_scatter_matrix;
291 overall_scatter_matrix.setZero ();
292 std::vector<float> total_weight (number_of_triangles);
293 const float denominator = 1.0f / 6.0f;
294 for (std::size_t i_triangle = 0; i_triangle < number_of_triangles; i_triangle++)
296 const float factor = distance_weight[i_triangle] * triangle_area[i_triangle] * total_area;
297 overall_scatter_matrix += factor * scatter_matrices[i_triangle];
298 total_weight[i_triangle] = factor * denominator;
301 Eigen::Vector3f v1, v2, v3;
302 computeEigenVectors (overall_scatter_matrix, v1, v2, v3);
306 std::size_t i_triangle = 0;
307 for (
const auto& triangle: local_triangles)
309 Eigen::Vector3f pt[3];
310 for (
unsigned int i_vertex = 0; i_vertex < 3; i_vertex++)
312 const unsigned int index = triangles_[triangle].vertices[i_vertex];
313 pt[i_vertex] (0) = (*surface_)[index].x;
314 pt[i_vertex] (1) = (*surface_)[index].y;
315 pt[i_vertex] (2) = (*surface_)[index].z;
318 float factor1 = 0.0f;
319 float factor3 = 0.0f;
320 for (
const auto &i_pt : pt)
322 Eigen::Vector3f vec = i_pt - feature_point;
323 factor1 += vec.dot (v1);
324 factor3 += vec.dot (v3);
326 h1 += total_weight[i_triangle] * factor1;
327 h3 += total_weight[i_triangle] * factor3;
331 if (h1 < 0.0f) v1 = -v1;
332 if (h3 < 0.0f) v3 = -v3;
336 lrf_matrix.row (0) = v1;
337 lrf_matrix.row (1) = v2;
338 lrf_matrix.row (2) = v3;
342template <
typename Po
intInT,
typename Po
intOutT>
void
343pcl::ROPSEstimation <PointInT, PointOutT>::computeEigenVectors (
const Eigen::Matrix3f& matrix,
344 Eigen::Vector3f& major_axis, Eigen::Vector3f& middle_axis, Eigen::Vector3f& minor_axis)
const
346 Eigen::EigenSolver <Eigen::Matrix3f> eigen_solver;
347 eigen_solver.compute (matrix);
349 Eigen::EigenSolver <Eigen::Matrix3f>::EigenvectorsType eigen_vectors;
350 Eigen::EigenSolver <Eigen::Matrix3f>::EigenvalueType eigen_values;
351 eigen_vectors = eigen_solver.eigenvectors ();
352 eigen_values = eigen_solver.eigenvalues ();
354 unsigned int temp = 0;
355 unsigned int major_index = 0;
356 unsigned int middle_index = 1;
357 unsigned int minor_index = 2;
359 if (eigen_values.real () (major_index) < eigen_values.real () (middle_index))
362 major_index = middle_index;
366 if (eigen_values.real () (major_index) < eigen_values.real () (minor_index))
369 major_index = minor_index;
373 if (eigen_values.real () (middle_index) < eigen_values.real () (minor_index))
376 minor_index = middle_index;
380 major_axis = eigen_vectors.col (major_index).real ();
381 middle_axis = eigen_vectors.col (middle_index).real ();
382 minor_axis = eigen_vectors.col (minor_index).real ();
386template <
typename Po
intInT,
typename Po
intOutT>
void
387pcl::ROPSEstimation <PointInT, PointOutT>::transformCloud (
const PointInT& point,
const Eigen::Matrix3f& matrix,
const pcl::Indices& local_points, PointCloudIn& transformed_cloud)
const
389 const auto number_of_points = local_points.size ();
390 transformed_cloud.clear ();
391 transformed_cloud.reserve (number_of_points);
393 for (
const auto& idx: local_points)
395 Eigen::Vector3f transformed_point ((*surface_)[idx].x - point.x,
396 (*surface_)[idx].y - point.y,
397 (*surface_)[idx].z - point.z);
399 transformed_point = matrix * transformed_point;
402 new_point.x = transformed_point (0);
403 new_point.y = transformed_point (1);
404 new_point.z = transformed_point (2);
405 transformed_cloud.emplace_back (new_point);
410template <
typename Po
intInT,
typename Po
intOutT>
void
411pcl::ROPSEstimation <PointInT, PointOutT>::rotateCloud (
const PointInT& axis,
const float angle,
const PointCloudIn& cloud, PointCloudIn& rotated_cloud, Eigen::Vector3f& min, Eigen::Vector3f& max)
const
413 Eigen::Matrix3f rotation_matrix;
414 const float x = axis.x;
415 const float y = axis.y;
416 const float z = axis.z;
417 const float rad =
M_PI / 180.0f;
418 const float cosine = std::cos (angle * rad);
419 const float sine = std::sin (angle * rad);
420 rotation_matrix << cosine + (1 - cosine) * x * x, (1 - cosine) * x * y - sine * z, (1 - cosine) * x * z + sine * y,
421 (1 - cosine) * y * x + sine * z, cosine + (1 - cosine) * y * y, (1 - cosine) * y * z - sine * x,
422 (1 - cosine) * z * x - sine * y, (1 - cosine) * z * y + sine * x, cosine + (1 - cosine) * z * z;
424 const auto number_of_points = cloud.size ();
426 rotated_cloud.header = cloud.header;
427 rotated_cloud.width = number_of_points;
428 rotated_cloud.height = 1;
429 rotated_cloud.clear ();
430 rotated_cloud.reserve (number_of_points);
432 min (0) = std::numeric_limits <float>::max ();
433 min (1) = std::numeric_limits <float>::max ();
434 min (2) = std::numeric_limits <float>::max ();
435 max (0) = -std::numeric_limits <float>::max ();
436 max (1) = -std::numeric_limits <float>::max ();
437 max (2) = -std::numeric_limits <float>::max ();
439 for (
const auto& pt: cloud.points)
441 Eigen::Vector3f point (pt.x, pt.y, pt.z);
442 point = rotation_matrix * point;
444 PointInT rotated_point;
445 rotated_point.x = point (0);
446 rotated_point.y = point (1);
447 rotated_point.z = point (2);
448 rotated_cloud.emplace_back (rotated_point);
450 for (
int i = 0; i < 3; ++i)
452 min(i) = std::min(min(i), point(i));
453 max(i) = std::max(max(i), point(i));
459template <
typename Po
intInT,
typename Po
intOutT>
void
460pcl::ROPSEstimation <PointInT, PointOutT>::getDistributionMatrix (
const unsigned int projection,
const Eigen::Vector3f& min,
const Eigen::Vector3f& max,
const PointCloudIn& cloud, Eigen::MatrixXf& matrix)
const
464 const unsigned int coord[3][2] = {
469 const float u_bin_length = (max (coord[projection][0]) - min (coord[projection][0])) / number_of_bins_;
470 const float v_bin_length = (max (coord[projection][1]) - min (coord[projection][1])) / number_of_bins_;
472 for (
const auto& pt: cloud.points)
474 Eigen::Vector3f point (pt.x, pt.y, pt.z);
476 const float u_length = point (coord[projection][0]) - min[coord[projection][0]];
477 const float v_length = point (coord[projection][1]) - min[coord[projection][1]];
479 const float u_ratio = u_length / u_bin_length;
480 auto row =
static_cast <unsigned int> (u_ratio);
481 if (row == number_of_bins_) row--;
483 const float v_ratio = v_length / v_bin_length;
484 auto col =
static_cast <unsigned int> (v_ratio);
485 if (col == number_of_bins_) col--;
487 matrix (row, col) += 1.0f;
490 matrix /= std::max<float> (1, cloud.size ());
494template <
typename Po
intInT,
typename Po
intOutT>
void
495pcl::ROPSEstimation <PointInT, PointOutT>::computeCentralMoments (
const Eigen::MatrixXf& matrix, std::vector <float>& moments)
const
500 for (
unsigned int i = 0; i < number_of_bins_; i++)
501 for (
unsigned int j = 0; j < number_of_bins_; j++)
503 const float m = matrix (i, j);
504 mean_i +=
static_cast <float> (i + 1) * m;
505 mean_j +=
static_cast <float> (j + 1) * m;
508 const unsigned int number_of_moments_to_compute = 4;
509 const float power[number_of_moments_to_compute][2] = {
515 float entropy = 0.0f;
516 moments.resize (number_of_moments_to_compute + 1, 0.0f);
517 for (
unsigned int i = 0; i < number_of_bins_; i++)
519 const float i_factor =
static_cast <float> (i + 1) - mean_i;
520 for (
unsigned int j = 0; j < number_of_bins_; j++)
522 const float j_factor =
static_cast <float> (j + 1) - mean_j;
523 const float m = matrix (i, j);
525 entropy -= m * std::log (m);
526 for (
unsigned int i_moment = 0; i_moment < number_of_moments_to_compute; i_moment++)
527 moments[i_moment] += std::pow (i_factor, power[i_moment][0]) * std::pow (j_factor, power[i_moment][1]) * m;
531 moments[number_of_moments_to_compute] = entropy;
float getSupportRadius() const
Returns the support radius.
void setNumberOfRotations(unsigned int number_of_rotations)
This method sets the number of rotations.
ROPSEstimation()
Simple constructor.
void setNumberOfPartitionBins(unsigned int number_of_bins)
Allows to set the number of partition bins that is used for distribution matrix calculation.
unsigned int getNumberOfPartitionBins() const
Returns the number of partition bins.
unsigned int getNumberOfRotations() const
returns the number of rotations.
void setSupportRadius(float support_radius)
Allows to set the support radius that is used to crop the local surface of the point.
void setTriangles(const std::vector< pcl::Vertices > &triangles)
This method sets the triangles of the mesh.
void getTriangles(std::vector< pcl::Vertices > &triangles) const
Returns the triangles of the mesh.
__device__ __host__ __forceinline__ float norm(const float3 &v1, const float3 &v2)
IndicesAllocator<> Indices
Type used for indices in PCL.