Point Cloud Library (PCL)  1.14.1-dev
rops_estimation.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  *
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Willow Garage, Inc. nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *
35  * Author : Sergey Ushakov
36  * Email : sergey.s.ushakov@mail.ru
37  *
38  */
39 
40 #ifndef PCL_ROPS_ESTIMATION_HPP_
41 #define PCL_ROPS_ESTIMATION_HPP_
42 
43 #include <pcl/features/rops_estimation.h>
44 
45 #include <array>
46 #include <numeric> // for accumulate
47 #include <Eigen/Eigenvalues> // for EigenSolver
48 
49 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
50 template <typename PointInT, typename PointOutT>
52 
53  triangles_ (0),
54  triangles_of_the_point_ (0)
55 {
56 }
57 
58 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
59 template <typename PointInT, typename PointOutT>
61 {
62  triangles_.clear ();
63  triangles_of_the_point_.clear ();
64 }
65 
66 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
67 template <typename PointInT, typename PointOutT> void
69 {
70  if (number_of_bins != 0)
71  number_of_bins_ = number_of_bins;
72 }
73 
74 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
75 template <typename PointInT, typename PointOutT> unsigned int
77 {
78  return (number_of_bins_);
79 }
80 
81 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
82 template <typename PointInT, typename PointOutT> void
84 {
85  if (number_of_rotations != 0)
86  {
87  number_of_rotations_ = number_of_rotations;
88  step_ = 90.0f / static_cast <float> (number_of_rotations_ + 1);
89  }
90 }
91 
92 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
93 template <typename PointInT, typename PointOutT> unsigned int
95 {
96  return (number_of_rotations_);
97 }
98 
99 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
100 template <typename PointInT, typename PointOutT> void
102 {
103  if (support_radius > 0.0f)
104  {
105  support_radius_ = support_radius;
106  sqr_support_radius_ = support_radius * support_radius;
107  }
108 }
109 
110 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
111 template <typename PointInT, typename PointOutT> float
113 {
114  return (support_radius_);
115 }
116 
117 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
118 template <typename PointInT, typename PointOutT> void
119 pcl::ROPSEstimation <PointInT, PointOutT>::setTriangles (const std::vector <pcl::Vertices>& triangles)
120 {
121  triangles_ = triangles;
122 }
123 
124 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
125 template <typename PointInT, typename PointOutT> void
126 pcl::ROPSEstimation <PointInT, PointOutT>::getTriangles (std::vector <pcl::Vertices>& triangles) const
127 {
128  triangles = triangles_;
129 }
130 
131 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
132 template <typename PointInT, typename PointOutT> void
134 {
135  if (triangles_.empty ())
136  {
137  output.clear ();
138  return;
139  }
140 
141  buildListOfPointsTriangles ();
142 
143  //feature size = number_of_rotations * number_of_axis_to_rotate_around * number_of_projections * number_of_central_moments
144  unsigned int feature_size = number_of_rotations_ * 3 * 3 * 5;
145  const auto number_of_points = indices_->size ();
146  output.clear ();
147  output.reserve (number_of_points);
148 
149  for (const auto& idx: *indices_)
150  {
151  std::set <unsigned int> local_triangles;
152  pcl::Indices local_points;
153  getLocalSurface ((*input_)[idx], local_triangles, local_points);
154 
155  Eigen::Matrix3f lrf_matrix;
156  computeLRF ((*input_)[idx], local_triangles, lrf_matrix);
157 
158  PointCloudIn transformed_cloud;
159  transformCloud ((*input_)[idx], lrf_matrix, local_points, transformed_cloud);
160 
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)
167  {
168  float theta = step_;
169  do
170  {
171  //rotate local surface and get bounding box
172  PointCloudIn rotated_cloud;
173  Eigen::Vector3f min, max;
174  rotateCloud (axis, theta, transformed_cloud, rotated_cloud, min, max);
175 
176  //for each projection (XY, XZ and YZ) compute distribution matrix and central moments
177  for (unsigned int i_proj = 0; i_proj < 3; i_proj++)
178  {
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);
182 
183  // TODO remove this needless copy due to API design
184  std::vector <float> moments;
185  computeCentralMoments (distribution_matrix, moments);
186 
187  feature.insert (feature.end (), moments.begin (), moments.end ());
188  }
189 
190  theta += step_;
191  } while (theta < 90.0f);
192  }
193 
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);
197  });
198  float invert_norm;
199  if (norm < std::numeric_limits <float>::epsilon ())
200  invert_norm = 1.0f;
201  else
202  invert_norm = 1.0f / norm;
203 
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;
207  }
208 }
209 
210 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
211 template <typename PointInT, typename PointOutT> void
213 {
214  triangles_of_the_point_.clear ();
215 
216  std::vector <unsigned int> dummy;
217  dummy.reserve (100);
218  triangles_of_the_point_.resize (surface_->points. size (), dummy);
219 
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);
223 }
224 
225 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
226 template <typename PointInT, typename PointOutT> void
227 pcl::ROPSEstimation <PointInT, PointOutT>::getLocalSurface (const PointInT& point, std::set <unsigned int>& local_triangles, pcl::Indices& local_points) const
228 {
229  std::vector <float> distances;
230  tree_->radiusSearch (point, support_radius_, local_points, distances);
231 
232  for (const auto& pt: local_points)
233  local_triangles.insert (triangles_of_the_point_[pt].begin (),
234  triangles_of_the_point_[pt].end ());
235 }
236 
237 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
238 template <typename PointInT, typename PointOutT> void
239 pcl::ROPSEstimation <PointInT, PointOutT>::computeLRF (const PointInT& point, const std::set <unsigned int>& local_triangles, Eigen::Matrix3f& lrf_matrix) const
240 {
241  std::size_t number_of_triangles = local_triangles.size ();
242 
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);
245 
246  scatter_matrices.reserve (number_of_triangles);
247  triangle_area.clear ();
248  distance_weight.clear ();
249 
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;
253 
254  Eigen::Vector3f feature_point (point.x, point.y, point.z);
255 
256  for (const auto& triangle: local_triangles)
257  {
258  Eigen::Vector3f pt[3];
259  for (unsigned int i_vertex = 0; i_vertex < 3; i_vertex++)
260  {
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;
265  }
266 
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;
270 
271  distance_weight.push_back (std::pow (support_radius_ - (feature_point - (pt[0] + pt[1] + pt[2]) * coeff_1_div_3).norm (), 2.0f));
272 
273  Eigen::Matrix3f curr_scatter_matrix;
274  curr_scatter_matrix.setZero ();
275  for (const auto &i_pt : pt)
276  {
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 ());
281  }
282  scatter_matrices.emplace_back (coeff * curr_scatter_matrix);
283  }
284 
285  if (std::abs (total_area) < std::numeric_limits <float>::epsilon ())
286  total_area = 1.0f / total_area;
287  else
288  total_area = 1.0f;
289 
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++)
295  {
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;
299  }
300 
301  Eigen::Vector3f v1, v2, v3;
302  computeEigenVectors (overall_scatter_matrix, v1, v2, v3);
303 
304  float h1 = 0.0f;
305  float h3 = 0.0f;
306  std::size_t i_triangle = 0;
307  for (const auto& triangle: local_triangles)
308  {
309  Eigen::Vector3f pt[3];
310  for (unsigned int i_vertex = 0; i_vertex < 3; i_vertex++)
311  {
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;
316  }
317 
318  float factor1 = 0.0f;
319  float factor3 = 0.0f;
320  for (const auto &i_pt : pt)
321  {
322  Eigen::Vector3f vec = i_pt - feature_point;
323  factor1 += vec.dot (v1);
324  factor3 += vec.dot (v3);
325  }
326  h1 += total_weight[i_triangle] * factor1;
327  h3 += total_weight[i_triangle] * factor3;
328  i_triangle++;
329  }
330 
331  if (h1 < 0.0f) v1 = -v1;
332  if (h3 < 0.0f) v3 = -v3;
333 
334  v2 = v3.cross (v1);
335 
336  lrf_matrix.row (0) = v1;
337  lrf_matrix.row (1) = v2;
338  lrf_matrix.row (2) = v3;
339 }
340 
341 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
342 template <typename PointInT, typename PointOutT> void
344  Eigen::Vector3f& major_axis, Eigen::Vector3f& middle_axis, Eigen::Vector3f& minor_axis) const
345 {
346  Eigen::EigenSolver <Eigen::Matrix3f> eigen_solver;
347  eigen_solver.compute (matrix);
348 
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 ();
353 
354  unsigned int temp = 0;
355  unsigned int major_index = 0;
356  unsigned int middle_index = 1;
357  unsigned int minor_index = 2;
358 
359  if (eigen_values.real () (major_index) < eigen_values.real () (middle_index))
360  {
361  temp = major_index;
362  major_index = middle_index;
363  middle_index = temp;
364  }
365 
366  if (eigen_values.real () (major_index) < eigen_values.real () (minor_index))
367  {
368  temp = major_index;
369  major_index = minor_index;
370  minor_index = temp;
371  }
372 
373  if (eigen_values.real () (middle_index) < eigen_values.real () (minor_index))
374  {
375  temp = minor_index;
376  minor_index = middle_index;
377  middle_index = temp;
378  }
379 
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 ();
383 }
384 
385 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
386 template <typename PointInT, typename PointOutT> void
387 pcl::ROPSEstimation <PointInT, PointOutT>::transformCloud (const PointInT& point, const Eigen::Matrix3f& matrix, const pcl::Indices& local_points, PointCloudIn& transformed_cloud) const
388 {
389  const auto number_of_points = local_points.size ();
390  transformed_cloud.clear ();
391  transformed_cloud.reserve (number_of_points);
392 
393  for (const auto& idx: local_points)
394  {
395  Eigen::Vector3f transformed_point ((*surface_)[idx].x - point.x,
396  (*surface_)[idx].y - point.y,
397  (*surface_)[idx].z - point.z);
398 
399  transformed_point = matrix * transformed_point;
400 
401  PointInT new_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);
406  }
407 }
408 
409 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
410 template <typename PointInT, typename PointOutT> void
411 pcl::ROPSEstimation <PointInT, PointOutT>::rotateCloud (const PointInT& axis, const float angle, const PointCloudIn& cloud, PointCloudIn& rotated_cloud, Eigen::Vector3f& min, Eigen::Vector3f& max) const
412 {
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;
423 
424  const auto number_of_points = cloud.size ();
425 
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);
431 
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 ();
438 
439  for (const auto& pt: cloud.points)
440  {
441  Eigen::Vector3f point (pt.x, pt.y, pt.z);
442  point = rotation_matrix * point;
443 
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);
449 
450  for (int i = 0; i < 3; ++i)
451  {
452  min(i) = std::min(min(i), point(i));
453  max(i) = std::max(max(i), point(i));
454  }
455  }
456 }
457 
458 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
459 template <typename PointInT, typename PointOutT> void
460 pcl::ROPSEstimation <PointInT, PointOutT>::getDistributionMatrix (const unsigned int projection, const Eigen::Vector3f& min, const Eigen::Vector3f& max, const PointCloudIn& cloud, Eigen::MatrixXf& matrix) const
461 {
462  matrix.setZero ();
463 
464  const unsigned int coord[3][2] = {
465  {0, 1},
466  {0, 2},
467  {1, 2}};
468 
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_;
471 
472  for (const auto& pt: cloud.points)
473  {
474  Eigen::Vector3f point (pt.x, pt.y, pt.z);
475 
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]];
478 
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--;
482 
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--;
486 
487  matrix (row, col) += 1.0f;
488  }
489 
490  matrix /= std::max<float> (1, cloud.size ());
491 }
492 
493 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
494 template <typename PointInT, typename PointOutT> void
495 pcl::ROPSEstimation <PointInT, PointOutT>::computeCentralMoments (const Eigen::MatrixXf& matrix, std::vector <float>& moments) const
496 {
497  float mean_i = 0.0f;
498  float mean_j = 0.0f;
499 
500  for (unsigned int i = 0; i < number_of_bins_; i++)
501  for (unsigned int j = 0; j < number_of_bins_; j++)
502  {
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;
506  }
507 
508  const unsigned int number_of_moments_to_compute = 4;
509  const float power[number_of_moments_to_compute][2] = {
510  {1.0f, 1.0f},
511  {2.0f, 1.0f},
512  {1.0f, 2.0f},
513  {2.0f, 2.0f}};
514 
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++)
518  {
519  const float i_factor = static_cast <float> (i + 1) - mean_i;
520  for (unsigned int j = 0; j < number_of_bins_; j++)
521  {
522  const float j_factor = static_cast <float> (j + 1) - mean_j;
523  const float m = matrix (i, j);
524  if (m > 0.0f)
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;
528  }
529  }
530 
531  moments[number_of_moments_to_compute] = entropy;
532 }
533 
534 #endif // PCL_ROPS_ESTIMATION_HPP_
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition: feature.hpp:194
This class implements the method for extracting RoPS features presented in the article "Rotational Pr...
__device__ __host__ __forceinline__ float norm(const float3 &v1, const float3 &v2)
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
#define M_PI
Definition: pcl_macros.h:201