Point Cloud Library (PCL)  1.11.0
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 
47 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
48 template <typename PointInT, typename PointOutT>
50  number_of_bins_ (5),
51  number_of_rotations_ (3),
52  support_radius_ (1.0f),
53  sqr_support_radius_ (1.0f),
54  step_ (22.5f),
55  triangles_ (0),
56  triangles_of_the_point_ (0)
57 {
58 }
59 
60 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
61 template <typename PointInT, typename PointOutT>
63 {
64  triangles_.clear ();
65  triangles_of_the_point_.clear ();
66 }
67 
68 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
69 template <typename PointInT, typename PointOutT> void
71 {
72  if (number_of_bins != 0)
73  number_of_bins_ = number_of_bins;
74 }
75 
76 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
77 template <typename PointInT, typename PointOutT> unsigned int
79 {
80  return (number_of_bins_);
81 }
82 
83 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
84 template <typename PointInT, typename PointOutT> void
86 {
87  if (number_of_rotations != 0)
88  {
89  number_of_rotations_ = number_of_rotations;
90  step_ = 90.0f / static_cast <float> (number_of_rotations_ + 1);
91  }
92 }
93 
94 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
95 template <typename PointInT, typename PointOutT> unsigned int
97 {
98  return (number_of_rotations_);
99 }
100 
101 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
102 template <typename PointInT, typename PointOutT> void
104 {
105  if (support_radius > 0.0f)
106  {
107  support_radius_ = support_radius;
108  sqr_support_radius_ = support_radius * support_radius;
109  }
110 }
111 
112 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
113 template <typename PointInT, typename PointOutT> float
115 {
116  return (support_radius_);
117 }
118 
119 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
120 template <typename PointInT, typename PointOutT> void
121 pcl::ROPSEstimation <PointInT, PointOutT>::setTriangles (const std::vector <pcl::Vertices>& triangles)
122 {
123  triangles_ = triangles;
124 }
125 
126 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
127 template <typename PointInT, typename PointOutT> void
128 pcl::ROPSEstimation <PointInT, PointOutT>::getTriangles (std::vector <pcl::Vertices>& triangles) const
129 {
130  triangles = triangles_;
131 }
132 
133 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
134 template <typename PointInT, typename PointOutT> void
136 {
137  if (triangles_.empty ())
138  {
139  output.points.clear ();
140  return;
141  }
142 
143  buildListOfPointsTriangles ();
144 
145  //feature size = number_of_rotations * number_of_axis_to_rotate_around * number_of_projections * number_of_central_moments
146  unsigned int feature_size = number_of_rotations_ * 3 * 3 * 5;
147  const auto number_of_points = indices_->size ();
148  output.points.clear ();
149  output.points.reserve (number_of_points);
150 
151  for (const auto& idx: *indices_)
152  {
153  std::set <unsigned int> local_triangles;
154  std::vector <int> local_points;
155  getLocalSurface (input_->points[idx], local_triangles, local_points);
156 
157  Eigen::Matrix3f lrf_matrix;
158  computeLRF (input_->points[idx], local_triangles, lrf_matrix);
159 
160  PointCloudIn transformed_cloud;
161  transformCloud (input_->points[idx], lrf_matrix, local_points, transformed_cloud);
162 
163  std::array<PointInT, 3> axes;
164  axes[0].x = 1.0f; axes[0].y = 0.0f; axes[0].z = 0.0f;
165  axes[1].x = 0.0f; axes[1].y = 1.0f; axes[1].z = 0.0f;
166  axes[2].x = 0.0f; axes[2].y = 0.0f; axes[2].z = 1.0f;
167  std::vector <float> feature;
168  for (const auto &axis : axes)
169  {
170  float theta = step_;
171  do
172  {
173  //rotate local surface and get bounding box
174  PointCloudIn rotated_cloud;
175  Eigen::Vector3f min, max;
176  rotateCloud (axis, theta, transformed_cloud, rotated_cloud, min, max);
177 
178  //for each projection (XY, XZ and YZ) compute distribution matrix and central moments
179  for (unsigned int i_proj = 0; i_proj < 3; i_proj++)
180  {
181  Eigen::MatrixXf distribution_matrix;
182  distribution_matrix.resize (number_of_bins_, number_of_bins_);
183  getDistributionMatrix (i_proj, min, max, rotated_cloud, distribution_matrix);
184 
185  // TODO remove this needless copy due to API design
186  std::vector <float> moments;
187  computeCentralMoments (distribution_matrix, moments);
188 
189  feature.insert (feature.end (), moments.begin (), moments.end ());
190  }
191 
192  theta += step_;
193  } while (theta < 90.0f);
194  }
195 
196  const float norm = std::accumulate(
197  feature.cbegin(), feature.cend(), 0.f, [](const auto& sum, const auto& val) {
198  return sum + std::abs(val);
199  });
200  float invert_norm;
201  if (norm < std::numeric_limits <float>::epsilon ())
202  invert_norm = 1.0f;
203  else
204  invert_norm = 1.0f / norm;
205 
206  output.points.emplace_back ();
207  for (std::size_t i_dim = 0; i_dim < feature_size; i_dim++)
208  output.points.back().histogram[i_dim] = feature[i_dim] * invert_norm;
209  }
210 }
211 
212 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
213 template <typename PointInT, typename PointOutT> void
215 {
216  triangles_of_the_point_.clear ();
217 
218  std::vector <unsigned int> dummy;
219  dummy.reserve (100);
220  triangles_of_the_point_.resize (surface_->points. size (), dummy);
221 
222  for (std::size_t i_triangle = 0; i_triangle < triangles_.size (); i_triangle++)
223  for (const auto& vertex: triangles_[i_triangle].vertices)
224  triangles_of_the_point_[vertex].push_back (i_triangle);
225 }
226 
227 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
228 template <typename PointInT, typename PointOutT> void
229 pcl::ROPSEstimation <PointInT, PointOutT>::getLocalSurface (const PointInT& point, std::set <unsigned int>& local_triangles, std::vector <int>& local_points) const
230 {
231  std::vector <float> distances;
232  tree_->radiusSearch (point, support_radius_, local_points, distances);
233 
234  for (const auto& pt: local_points)
235  local_triangles.insert (triangles_of_the_point_[pt].begin (),
236  triangles_of_the_point_[pt].end ());
237 }
238 
239 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
240 template <typename PointInT, typename PointOutT> void
241 pcl::ROPSEstimation <PointInT, PointOutT>::computeLRF (const PointInT& point, const std::set <unsigned int>& local_triangles, Eigen::Matrix3f& lrf_matrix) const
242 {
243  std::size_t number_of_triangles = local_triangles.size ();
244 
245  std::vector<Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > scatter_matrices;
246  std::vector <float> triangle_area (number_of_triangles), distance_weight (number_of_triangles);
247 
248  scatter_matrices.reserve (number_of_triangles);
249  triangle_area.clear ();
250  distance_weight.clear ();
251 
252  float total_area = 0.0f;
253  const float coeff = 1.0f / 12.0f;
254  const float coeff_1_div_3 = 1.0f / 3.0f;
255 
256  Eigen::Vector3f feature_point (point.x, point.y, point.z);
257 
258  for (const auto& triangle: local_triangles)
259  {
260  Eigen::Vector3f pt[3];
261  for (unsigned int i_vertex = 0; i_vertex < 3; i_vertex++)
262  {
263  const unsigned int index = triangles_[triangle].vertices[i_vertex];
264  pt[i_vertex] (0) = surface_->points[index].x;
265  pt[i_vertex] (1) = surface_->points[index].y;
266  pt[i_vertex] (2) = surface_->points[index].z;
267  }
268 
269  const float curr_area = ((pt[1] - pt[0]).cross (pt[2] - pt[0])).norm ();
270  triangle_area.push_back (curr_area);
271  total_area += curr_area;
272 
273  distance_weight.push_back (std::pow (support_radius_ - (feature_point - (pt[0] + pt[1] + pt[2]) * coeff_1_div_3).norm (), 2.0f));
274 
275  Eigen::Matrix3f curr_scatter_matrix;
276  curr_scatter_matrix.setZero ();
277  for (const auto &i_pt : pt)
278  {
279  Eigen::Vector3f vec = i_pt - feature_point;
280  curr_scatter_matrix += vec * (vec.transpose ());
281  for (const auto &j_pt : pt)
282  curr_scatter_matrix += vec * ((j_pt - feature_point).transpose ());
283  }
284  scatter_matrices.emplace_back (coeff * curr_scatter_matrix);
285  }
286 
287  if (std::abs (total_area) < std::numeric_limits <float>::epsilon ())
288  total_area = 1.0f / total_area;
289  else
290  total_area = 1.0f;
291 
292  Eigen::Matrix3f overall_scatter_matrix;
293  overall_scatter_matrix.setZero ();
294  std::vector<float> total_weight (number_of_triangles);
295  const float denominator = 1.0f / 6.0f;
296  for (std::size_t i_triangle = 0; i_triangle < number_of_triangles; i_triangle++)
297  {
298  const float factor = distance_weight[i_triangle] * triangle_area[i_triangle] * total_area;
299  overall_scatter_matrix += factor * scatter_matrices[i_triangle];
300  total_weight[i_triangle] = factor * denominator;
301  }
302 
303  Eigen::Vector3f v1, v2, v3;
304  computeEigenVectors (overall_scatter_matrix, v1, v2, v3);
305 
306  float h1 = 0.0f;
307  float h3 = 0.0f;
308  std::size_t i_triangle = 0;
309  for (const auto& triangle: local_triangles)
310  {
311  Eigen::Vector3f pt[3];
312  for (unsigned int i_vertex = 0; i_vertex < 3; i_vertex++)
313  {
314  const unsigned int index = triangles_[triangle].vertices[i_vertex];
315  pt[i_vertex] (0) = surface_->points[index].x;
316  pt[i_vertex] (1) = surface_->points[index].y;
317  pt[i_vertex] (2) = surface_->points[index].z;
318  }
319 
320  float factor1 = 0.0f;
321  float factor3 = 0.0f;
322  for (const auto &i_pt : pt)
323  {
324  Eigen::Vector3f vec = i_pt - feature_point;
325  factor1 += vec.dot (v1);
326  factor3 += vec.dot (v3);
327  }
328  h1 += total_weight[i_triangle] * factor1;
329  h3 += total_weight[i_triangle] * factor3;
330  i_triangle++;
331  }
332 
333  if (h1 < 0.0f) v1 = -v1;
334  if (h3 < 0.0f) v3 = -v3;
335 
336  v2 = v3.cross (v1);
337 
338  lrf_matrix.row (0) = v1;
339  lrf_matrix.row (1) = v2;
340  lrf_matrix.row (2) = v3;
341 }
342 
343 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
344 template <typename PointInT, typename PointOutT> void
346  Eigen::Vector3f& major_axis, Eigen::Vector3f& middle_axis, Eigen::Vector3f& minor_axis) const
347 {
348  Eigen::EigenSolver <Eigen::Matrix3f> eigen_solver;
349  eigen_solver.compute (matrix);
350 
351  Eigen::EigenSolver <Eigen::Matrix3f>::EigenvectorsType eigen_vectors;
352  Eigen::EigenSolver <Eigen::Matrix3f>::EigenvalueType eigen_values;
353  eigen_vectors = eigen_solver.eigenvectors ();
354  eigen_values = eigen_solver.eigenvalues ();
355 
356  unsigned int temp = 0;
357  unsigned int major_index = 0;
358  unsigned int middle_index = 1;
359  unsigned int minor_index = 2;
360 
361  if (eigen_values.real () (major_index) < eigen_values.real () (middle_index))
362  {
363  temp = major_index;
364  major_index = middle_index;
365  middle_index = temp;
366  }
367 
368  if (eigen_values.real () (major_index) < eigen_values.real () (minor_index))
369  {
370  temp = major_index;
371  major_index = minor_index;
372  minor_index = temp;
373  }
374 
375  if (eigen_values.real () (middle_index) < eigen_values.real () (minor_index))
376  {
377  temp = minor_index;
378  minor_index = middle_index;
379  middle_index = temp;
380  }
381 
382  major_axis = eigen_vectors.col (major_index).real ();
383  middle_axis = eigen_vectors.col (middle_index).real ();
384  minor_axis = eigen_vectors.col (minor_index).real ();
385 }
386 
387 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
388 template <typename PointInT, typename PointOutT> void
389 pcl::ROPSEstimation <PointInT, PointOutT>::transformCloud (const PointInT& point, const Eigen::Matrix3f& matrix, const std::vector <int>& local_points, PointCloudIn& transformed_cloud) const
390 {
391  const auto number_of_points = local_points.size ();
392  transformed_cloud.points.clear ();
393  transformed_cloud.points.reserve (number_of_points);
394 
395  for (const auto& idx: local_points)
396  {
397  Eigen::Vector3f transformed_point (surface_->points[idx].x - point.x,
398  surface_->points[idx].y - point.y,
399  surface_->points[idx].z - point.z);
400 
401  transformed_point = matrix * transformed_point;
402 
403  PointInT new_point;
404  new_point.x = transformed_point (0);
405  new_point.y = transformed_point (1);
406  new_point.z = transformed_point (2);
407  transformed_cloud.points.emplace_back (new_point);
408  }
409 }
410 
411 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
412 template <typename PointInT, typename PointOutT> void
413 pcl::ROPSEstimation <PointInT, PointOutT>::rotateCloud (const PointInT& axis, const float angle, const PointCloudIn& cloud, PointCloudIn& rotated_cloud, Eigen::Vector3f& min, Eigen::Vector3f& max) const
414 {
415  Eigen::Matrix3f rotation_matrix;
416  const float x = axis.x;
417  const float y = axis.y;
418  const float z = axis.z;
419  const float rad = M_PI / 180.0f;
420  const float cosine = std::cos (angle * rad);
421  const float sine = std::sin (angle * rad);
422  rotation_matrix << cosine + (1 - cosine) * x * x, (1 - cosine) * x * y - sine * z, (1 - cosine) * x * z + sine * y,
423  (1 - cosine) * y * x + sine * z, cosine + (1 - cosine) * y * y, (1 - cosine) * y * z - sine * x,
424  (1 - cosine) * z * x - sine * y, (1 - cosine) * z * y + sine * x, cosine + (1 - cosine) * z * z;
425 
426  const auto number_of_points = cloud.points.size ();
427 
428  rotated_cloud.header = cloud.header;
429  rotated_cloud.width = number_of_points;
430  rotated_cloud.height = 1;
431  rotated_cloud.points.clear ();
432  rotated_cloud.points.reserve (number_of_points);
433 
434  min (0) = std::numeric_limits <float>::max ();
435  min (1) = std::numeric_limits <float>::max ();
436  min (2) = std::numeric_limits <float>::max ();
437  max (0) = -std::numeric_limits <float>::max ();
438  max (1) = -std::numeric_limits <float>::max ();
439  max (2) = -std::numeric_limits <float>::max ();
440 
441  for (const auto& pt: cloud.points)
442  {
443  Eigen::Vector3f point (pt.x, pt.y, pt.z);
444  point = rotation_matrix * point;
445 
446  PointInT rotated_point;
447  rotated_point.x = point (0);
448  rotated_point.y = point (1);
449  rotated_point.z = point (2);
450  rotated_cloud.points.emplace_back (rotated_point);
451 
452  for (int i = 0; i < 3; ++i)
453  {
454  min(i) = std::min(min(i), point(i));
455  max(i) = std::max(max(i), point(i));
456  }
457  }
458 }
459 
460 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
461 template <typename PointInT, typename PointOutT> void
462 pcl::ROPSEstimation <PointInT, PointOutT>::getDistributionMatrix (const unsigned int projection, const Eigen::Vector3f& min, const Eigen::Vector3f& max, const PointCloudIn& cloud, Eigen::MatrixXf& matrix) const
463 {
464  matrix.setZero ();
465 
466  const unsigned int coord[3][2] = {
467  {0, 1},
468  {0, 2},
469  {1, 2}};
470 
471  const float u_bin_length = (max (coord[projection][0]) - min (coord[projection][0])) / number_of_bins_;
472  const float v_bin_length = (max (coord[projection][1]) - min (coord[projection][1])) / number_of_bins_;
473 
474  for (const auto& pt: cloud.points)
475  {
476  Eigen::Vector3f point (pt.x, pt.y, pt.z);
477 
478  const float u_length = point (coord[projection][0]) - min[coord[projection][0]];
479  const float v_length = point (coord[projection][1]) - min[coord[projection][1]];
480 
481  const float u_ratio = u_length / u_bin_length;
482  unsigned int row = static_cast <unsigned int> (u_ratio);
483  if (row == number_of_bins_) row--;
484 
485  const float v_ratio = v_length / v_bin_length;
486  unsigned int col = static_cast <unsigned int> (v_ratio);
487  if (col == number_of_bins_) col--;
488 
489  matrix (row, col) += 1.0f;
490  }
491 
492  matrix /= std::max<float> (1, cloud.points.size ());
493 }
494 
495 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
496 template <typename PointInT, typename PointOutT> void
497 pcl::ROPSEstimation <PointInT, PointOutT>::computeCentralMoments (const Eigen::MatrixXf& matrix, std::vector <float>& moments) const
498 {
499  float mean_i = 0.0f;
500  float mean_j = 0.0f;
501 
502  for (unsigned int i = 0; i < number_of_bins_; i++)
503  for (unsigned int j = 0; j < number_of_bins_; j++)
504  {
505  const float m = matrix (i, j);
506  mean_i += static_cast <float> (i + 1) * m;
507  mean_j += static_cast <float> (j + 1) * m;
508  }
509 
510  const unsigned int number_of_moments_to_compute = 4;
511  const float power[number_of_moments_to_compute][2] = {
512  {1.0f, 1.0f},
513  {2.0f, 1.0f},
514  {1.0f, 2.0f},
515  {2.0f, 2.0f}};
516 
517  float entropy = 0.0f;
518  moments.resize (number_of_moments_to_compute + 1, 0.0f);
519  for (unsigned int i = 0; i < number_of_bins_; i++)
520  {
521  const float i_factor = static_cast <float> (i + 1) - mean_i;
522  for (unsigned int j = 0; j < number_of_bins_; j++)
523  {
524  const float j_factor = static_cast <float> (j + 1) - mean_j;
525  const float m = matrix (i, j);
526  if (m > 0.0f)
527  entropy -= m * std::log (m);
528  for (unsigned int i_moment = 0; i_moment < number_of_moments_to_compute; i_moment++)
529  moments[i_moment] += std::pow (i_factor, power[i_moment][0]) * std::pow (j_factor, power[i_moment][1]) * m;
530  }
531  }
532 
533  moments[number_of_moments_to_compute] = entropy;
534 }
535 
536 #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:193
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)
#define M_PI
Definition: pcl_macros.h:195