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>
53 number_of_rotations_ (3),
54 support_radius_ (1.0f),
55 sqr_support_radius_ (1.0f),
58 triangles_of_the_point_ (0)
63template <
typename Po
intInT,
typename Po
intOutT>
64pcl::ROPSEstimation <PointInT, PointOutT>::~ROPSEstimation ()
67 triangles_of_the_point_.clear ();
71template <
typename Po
intInT,
typename Po
intOutT>
void
74 if (number_of_bins != 0)
75 number_of_bins_ = number_of_bins;
79template <
typename Po
intInT,
typename Po
intOutT>
unsigned int
82 return (number_of_bins_);
86template <
typename Po
intInT,
typename Po
intOutT>
void
89 if (number_of_rotations != 0)
91 number_of_rotations_ = number_of_rotations;
92 step_ = 90.0f /
static_cast <float> (number_of_rotations_ + 1);
97template <
typename Po
intInT,
typename Po
intOutT>
unsigned int
100 return (number_of_rotations_);
104template <
typename Po
intInT,
typename Po
intOutT>
void
107 if (support_radius > 0.0f)
109 support_radius_ = support_radius;
110 sqr_support_radius_ = support_radius * support_radius;
115template <
typename Po
intInT,
typename Po
intOutT>
float
118 return (support_radius_);
122template <
typename Po
intInT,
typename Po
intOutT>
void
125 triangles_ = triangles;
129template <
typename Po
intInT,
typename Po
intOutT>
void
132 triangles = triangles_;
136template <
typename Po
intInT,
typename Po
intOutT>
void
137pcl::ROPSEstimation <PointInT, PointOutT>::computeFeature (PointCloudOut &output)
139 if (triangles_.empty ())
145 buildListOfPointsTriangles ();
148 unsigned int feature_size = number_of_rotations_ * 3 * 3 * 5;
149 const auto number_of_points = indices_->size ();
151 output.reserve (number_of_points);
153 for (
const auto& idx: *indices_)
155 std::set <unsigned int> local_triangles;
157 getLocalSurface ((*input_)[idx], local_triangles, local_points);
159 Eigen::Matrix3f lrf_matrix;
160 computeLRF ((*input_)[idx], local_triangles, lrf_matrix);
162 PointCloudIn transformed_cloud;
163 transformCloud ((*input_)[idx], lrf_matrix, local_points, transformed_cloud);
165 std::array<PointInT, 3> axes;
166 axes[0].x = 1.0f; axes[0].y = 0.0f; axes[0].z = 0.0f;
167 axes[1].x = 0.0f; axes[1].y = 1.0f; axes[1].z = 0.0f;
168 axes[2].x = 0.0f; axes[2].y = 0.0f; axes[2].z = 1.0f;
169 std::vector <float> feature;
170 for (
const auto &axis : axes)
176 PointCloudIn rotated_cloud;
177 Eigen::Vector3f min, max;
178 rotateCloud (axis, theta, transformed_cloud, rotated_cloud, min, max);
181 for (
unsigned int i_proj = 0; i_proj < 3; i_proj++)
183 Eigen::MatrixXf distribution_matrix;
184 distribution_matrix.resize (number_of_bins_, number_of_bins_);
185 getDistributionMatrix (i_proj, min, max, rotated_cloud, distribution_matrix);
188 std::vector <float> moments;
189 computeCentralMoments (distribution_matrix, moments);
191 feature.insert (feature.end (), moments.begin (), moments.end ());
195 }
while (theta < 90.0f);
198 const float norm = std::accumulate(
199 feature.cbegin(), feature.cend(), 0.f, [](
const auto& sum,
const auto& val) {
200 return sum + std::abs(val);
203 if (norm < std::numeric_limits <float>::epsilon ())
206 invert_norm = 1.0f /
norm;
208 output.emplace_back ();
209 for (std::size_t i_dim = 0; i_dim < feature_size; i_dim++)
210 output.back().histogram[i_dim] = feature[i_dim] * invert_norm;
215template <
typename Po
intInT,
typename Po
intOutT>
void
216pcl::ROPSEstimation <PointInT, PointOutT>::buildListOfPointsTriangles ()
218 triangles_of_the_point_.clear ();
220 std::vector <unsigned int> dummy;
222 triangles_of_the_point_.resize (surface_->points. size (), dummy);
224 for (std::size_t i_triangle = 0; i_triangle < triangles_.size (); i_triangle++)
225 for (
const auto& vertex: triangles_[i_triangle].vertices)
226 triangles_of_the_point_[vertex].push_back (i_triangle);
230template <
typename Po
intInT,
typename Po
intOutT>
void
231pcl::ROPSEstimation <PointInT, PointOutT>::getLocalSurface (
const PointInT& point, std::set <unsigned int>& local_triangles,
pcl::Indices& local_points)
const
233 std::vector <float> distances;
234 tree_->radiusSearch (point, support_radius_, local_points, distances);
236 for (
const auto& pt: local_points)
237 local_triangles.insert (triangles_of_the_point_[pt].begin (),
238 triangles_of_the_point_[pt].end ());
242template <
typename Po
intInT,
typename Po
intOutT>
void
243pcl::ROPSEstimation <PointInT, PointOutT>::computeLRF (
const PointInT& point,
const std::set <unsigned int>& local_triangles, Eigen::Matrix3f& lrf_matrix)
const
245 std::size_t number_of_triangles = local_triangles.size ();
247 std::vector<Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > scatter_matrices;
248 std::vector <float> triangle_area (number_of_triangles), distance_weight (number_of_triangles);
250 scatter_matrices.reserve (number_of_triangles);
251 triangle_area.clear ();
252 distance_weight.clear ();
254 float total_area = 0.0f;
255 const float coeff = 1.0f / 12.0f;
256 const float coeff_1_div_3 = 1.0f / 3.0f;
258 Eigen::Vector3f feature_point (point.x, point.y, point.z);
260 for (
const auto& triangle: local_triangles)
262 Eigen::Vector3f pt[3];
263 for (
unsigned int i_vertex = 0; i_vertex < 3; i_vertex++)
265 const unsigned int index = triangles_[triangle].vertices[i_vertex];
266 pt[i_vertex] (0) = (*surface_)[index].x;
267 pt[i_vertex] (1) = (*surface_)[index].y;
268 pt[i_vertex] (2) = (*surface_)[index].z;
271 const float curr_area = ((pt[1] - pt[0]).cross (pt[2] - pt[0])).norm ();
272 triangle_area.push_back (curr_area);
273 total_area += curr_area;
275 distance_weight.push_back (std::pow (support_radius_ - (feature_point - (pt[0] + pt[1] + pt[2]) * coeff_1_div_3).norm (), 2.0f));
277 Eigen::Matrix3f curr_scatter_matrix;
278 curr_scatter_matrix.setZero ();
279 for (
const auto &i_pt : pt)
281 Eigen::Vector3f vec = i_pt - feature_point;
282 curr_scatter_matrix += vec * (vec.transpose ());
283 for (
const auto &j_pt : pt)
284 curr_scatter_matrix += vec * ((j_pt - feature_point).transpose ());
286 scatter_matrices.emplace_back (coeff * curr_scatter_matrix);
289 if (std::abs (total_area) < std::numeric_limits <float>::epsilon ())
290 total_area = 1.0f / total_area;
294 Eigen::Matrix3f overall_scatter_matrix;
295 overall_scatter_matrix.setZero ();
296 std::vector<float> total_weight (number_of_triangles);
297 const float denominator = 1.0f / 6.0f;
298 for (std::size_t i_triangle = 0; i_triangle < number_of_triangles; i_triangle++)
300 const float factor = distance_weight[i_triangle] * triangle_area[i_triangle] * total_area;
301 overall_scatter_matrix += factor * scatter_matrices[i_triangle];
302 total_weight[i_triangle] = factor * denominator;
305 Eigen::Vector3f v1, v2, v3;
306 computeEigenVectors (overall_scatter_matrix, v1, v2, v3);
310 std::size_t i_triangle = 0;
311 for (
const auto& triangle: local_triangles)
313 Eigen::Vector3f pt[3];
314 for (
unsigned int i_vertex = 0; i_vertex < 3; i_vertex++)
316 const unsigned int index = triangles_[triangle].vertices[i_vertex];
317 pt[i_vertex] (0) = (*surface_)[index].x;
318 pt[i_vertex] (1) = (*surface_)[index].y;
319 pt[i_vertex] (2) = (*surface_)[index].z;
322 float factor1 = 0.0f;
323 float factor3 = 0.0f;
324 for (
const auto &i_pt : pt)
326 Eigen::Vector3f vec = i_pt - feature_point;
327 factor1 += vec.dot (v1);
328 factor3 += vec.dot (v3);
330 h1 += total_weight[i_triangle] * factor1;
331 h3 += total_weight[i_triangle] * factor3;
335 if (h1 < 0.0f) v1 = -v1;
336 if (h3 < 0.0f) v3 = -v3;
340 lrf_matrix.row (0) = v1;
341 lrf_matrix.row (1) = v2;
342 lrf_matrix.row (2) = v3;
346template <
typename Po
intInT,
typename Po
intOutT>
void
347pcl::ROPSEstimation <PointInT, PointOutT>::computeEigenVectors (
const Eigen::Matrix3f& matrix,
348 Eigen::Vector3f& major_axis, Eigen::Vector3f& middle_axis, Eigen::Vector3f& minor_axis)
const
350 Eigen::EigenSolver <Eigen::Matrix3f> eigen_solver;
351 eigen_solver.compute (matrix);
353 Eigen::EigenSolver <Eigen::Matrix3f>::EigenvectorsType eigen_vectors;
354 Eigen::EigenSolver <Eigen::Matrix3f>::EigenvalueType eigen_values;
355 eigen_vectors = eigen_solver.eigenvectors ();
356 eigen_values = eigen_solver.eigenvalues ();
358 unsigned int temp = 0;
359 unsigned int major_index = 0;
360 unsigned int middle_index = 1;
361 unsigned int minor_index = 2;
363 if (eigen_values.real () (major_index) < eigen_values.real () (middle_index))
366 major_index = middle_index;
370 if (eigen_values.real () (major_index) < eigen_values.real () (minor_index))
373 major_index = minor_index;
377 if (eigen_values.real () (middle_index) < eigen_values.real () (minor_index))
380 minor_index = middle_index;
384 major_axis = eigen_vectors.col (major_index).real ();
385 middle_axis = eigen_vectors.col (middle_index).real ();
386 minor_axis = eigen_vectors.col (minor_index).real ();
390template <
typename Po
intInT,
typename Po
intOutT>
void
391pcl::ROPSEstimation <PointInT, PointOutT>::transformCloud (
const PointInT& point,
const Eigen::Matrix3f& matrix,
const pcl::Indices& local_points, PointCloudIn& transformed_cloud)
const
393 const auto number_of_points = local_points.size ();
394 transformed_cloud.clear ();
395 transformed_cloud.reserve (number_of_points);
397 for (
const auto& idx: local_points)
399 Eigen::Vector3f transformed_point ((*surface_)[idx].x - point.x,
400 (*surface_)[idx].y - point.y,
401 (*surface_)[idx].z - point.z);
403 transformed_point = matrix * transformed_point;
406 new_point.x = transformed_point (0);
407 new_point.y = transformed_point (1);
408 new_point.z = transformed_point (2);
409 transformed_cloud.emplace_back (new_point);
414template <
typename Po
intInT,
typename Po
intOutT>
void
415pcl::ROPSEstimation <PointInT, PointOutT>::rotateCloud (
const PointInT& axis,
const float angle,
const PointCloudIn& cloud, PointCloudIn& rotated_cloud, Eigen::Vector3f& min, Eigen::Vector3f& max)
const
417 Eigen::Matrix3f rotation_matrix;
418 const float x = axis.x;
419 const float y = axis.y;
420 const float z = axis.z;
421 const float rad =
M_PI / 180.0f;
422 const float cosine = std::cos (angle * rad);
423 const float sine = std::sin (angle * rad);
424 rotation_matrix << cosine + (1 - cosine) * x * x, (1 - cosine) * x * y - sine * z, (1 - cosine) * x * z + sine * y,
425 (1 - cosine) * y * x + sine * z, cosine + (1 - cosine) * y * y, (1 - cosine) * y * z - sine * x,
426 (1 - cosine) * z * x - sine * y, (1 - cosine) * z * y + sine * x, cosine + (1 - cosine) * z * z;
428 const auto number_of_points = cloud.size ();
430 rotated_cloud.header = cloud.header;
431 rotated_cloud.width = number_of_points;
432 rotated_cloud.height = 1;
433 rotated_cloud.clear ();
434 rotated_cloud.reserve (number_of_points);
436 min (0) = std::numeric_limits <float>::max ();
437 min (1) = std::numeric_limits <float>::max ();
438 min (2) = std::numeric_limits <float>::max ();
439 max (0) = -std::numeric_limits <float>::max ();
440 max (1) = -std::numeric_limits <float>::max ();
441 max (2) = -std::numeric_limits <float>::max ();
443 for (
const auto& pt: cloud.points)
445 Eigen::Vector3f point (pt.x, pt.y, pt.z);
446 point = rotation_matrix * point;
448 PointInT rotated_point;
449 rotated_point.x = point (0);
450 rotated_point.y = point (1);
451 rotated_point.z = point (2);
452 rotated_cloud.emplace_back (rotated_point);
454 for (
int i = 0; i < 3; ++i)
456 min(i) = std::min(min(i), point(i));
457 max(i) = std::max(max(i), point(i));
463template <
typename Po
intInT,
typename Po
intOutT>
void
464pcl::ROPSEstimation <PointInT, PointOutT>::getDistributionMatrix (
const unsigned int projection,
const Eigen::Vector3f& min,
const Eigen::Vector3f& max,
const PointCloudIn& cloud, Eigen::MatrixXf& matrix)
const
468 const unsigned int coord[3][2] = {
473 const float u_bin_length = (max (coord[projection][0]) - min (coord[projection][0])) / number_of_bins_;
474 const float v_bin_length = (max (coord[projection][1]) - min (coord[projection][1])) / number_of_bins_;
476 for (
const auto& pt: cloud.points)
478 Eigen::Vector3f point (pt.x, pt.y, pt.z);
480 const float u_length = point (coord[projection][0]) - min[coord[projection][0]];
481 const float v_length = point (coord[projection][1]) - min[coord[projection][1]];
483 const float u_ratio = u_length / u_bin_length;
484 auto row =
static_cast <unsigned int> (u_ratio);
485 if (row == number_of_bins_) row--;
487 const float v_ratio = v_length / v_bin_length;
488 auto col =
static_cast <unsigned int> (v_ratio);
489 if (col == number_of_bins_) col--;
491 matrix (row, col) += 1.0f;
494 matrix /= std::max<float> (1, cloud.size ());
498template <
typename Po
intInT,
typename Po
intOutT>
void
499pcl::ROPSEstimation <PointInT, PointOutT>::computeCentralMoments (
const Eigen::MatrixXf& matrix, std::vector <float>& moments)
const
504 for (
unsigned int i = 0; i < number_of_bins_; i++)
505 for (
unsigned int j = 0; j < number_of_bins_; j++)
507 const float m = matrix (i, j);
508 mean_i +=
static_cast <float> (i + 1) * m;
509 mean_j +=
static_cast <float> (j + 1) * m;
512 const unsigned int number_of_moments_to_compute = 4;
513 const float power[number_of_moments_to_compute][2] = {
519 float entropy = 0.0f;
520 moments.resize (number_of_moments_to_compute + 1, 0.0f);
521 for (
unsigned int i = 0; i < number_of_bins_; i++)
523 const float i_factor =
static_cast <float> (i + 1) - mean_i;
524 for (
unsigned int j = 0; j < number_of_bins_; j++)
526 const float j_factor =
static_cast <float> (j + 1) - mean_j;
527 const float m = matrix (i, j);
529 entropy -= m * std::log (m);
530 for (
unsigned int i_moment = 0; i_moment < number_of_moments_to_compute; i_moment++)
531 moments[i_moment] += std::pow (i_factor, power[i_moment][0]) * std::pow (j_factor, power[i_moment][1]) * m;
535 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.