Point Cloud Library (PCL)  1.7.2
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 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
46 template <typename PointInT, typename PointOutT>
48  number_of_bins_ (5),
49  number_of_rotations_ (3),
50  support_radius_ (1.0f),
51  sqr_support_radius_ (1.0f),
52  step_ (30.0f),
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_.size () == 0)
136  {
137  output.points.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  unsigned int number_of_points = static_cast <unsigned int> (indices_->size ());
146  output.points.resize (number_of_points, PointOutT ());
147 
148  for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
149  {
150  std::set <unsigned int> local_triangles;
151  std::vector <int> local_points;
152  getLocalSurface (input_->points[(*indices_)[i_point]], local_triangles, local_points);
153 
154  Eigen::Matrix3f lrf_matrix;
155  computeLRF (input_->points[(*indices_)[i_point]], local_triangles, lrf_matrix);
156 
157  PointCloudIn transformed_cloud;
158  transformCloud (input_->points[(*indices_)[i_point]], lrf_matrix, local_points, transformed_cloud);
159 
160  PointInT axis[3];
161  axis[0].x = 1.0f; axis[0].y = 0.0f; axis[0].z = 0.0f;
162  axis[1].x = 0.0f; axis[1].y = 1.0f; axis[1].z = 0.0f;
163  axis[2].x = 0.0f; axis[2].y = 0.0f; axis[2].z = 1.0f;
164  std::vector <float> feature;
165  for (unsigned int i_axis = 0; i_axis < 3; i_axis++)
166  {
167  float theta = step_;
168  do
169  {
170  //rotate local surface and get bounding box
171  PointCloudIn rotated_cloud;
172  Eigen::Vector3f min, max;
173  rotateCloud (axis[i_axis], theta, transformed_cloud, rotated_cloud, min, max);
174 
175  //for each projection (XY, XZ and YZ) compute distribution matrix and central moments
176  for (unsigned int i_proj = 0; i_proj < 3; i_proj++)
177  {
178  Eigen::MatrixXf distribution_matrix;
179  distribution_matrix.resize (number_of_bins_, number_of_bins_);
180  getDistributionMatrix (i_proj, min, max, rotated_cloud, distribution_matrix);
181 
182  std::vector <float> moments;
183  computeCentralMoments (distribution_matrix, moments);
184 
185  feature.insert (feature.end (), moments.begin (), moments.end ());
186  }
187 
188  theta += step_;
189  } while (theta < 90.0f);
190  }
191 
192  float norm = 0.0f;
193  for (unsigned int i_dim = 0; i_dim < feature_size; i_dim++)
194  norm += feature[i_dim];
195  if (abs (norm) < std::numeric_limits <float>::epsilon ())
196  norm = 1.0f / norm;
197  else
198  norm = 1.0f;
199 
200  for (unsigned int i_dim = 0; i_dim < feature_size; i_dim++)
201  output.points[i_point].histogram[i_dim] = feature[i_dim] * norm;
202  }
203 }
204 
205 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
206 template <typename PointInT, typename PointOutT> void
208 {
209  triangles_of_the_point_.clear ();
210 
211  const unsigned int number_of_triangles = static_cast <unsigned int> (triangles_.size ());
212 
213  std::vector <unsigned int> dummy;
214  dummy.reserve (100);
215  triangles_of_the_point_.resize (surface_->points. size (), dummy);
216 
217  for (unsigned int i_triangle = 0; i_triangle < number_of_triangles; i_triangle++)
218  for (unsigned int i_vertex = 0; i_vertex < 3; i_vertex++)
219  triangles_of_the_point_[triangles_[i_triangle].vertices[i_vertex]].push_back (i_triangle);
220 }
221 
222 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
223 template <typename PointInT, typename PointOutT> void
224 pcl::ROPSEstimation <PointInT, PointOutT>::getLocalSurface (const PointInT& point, std::set <unsigned int>& local_triangles, std::vector <int>& local_points) const
225 {
226  std::vector <float> distances;
227  tree_->radiusSearch (point, support_radius_, local_points, distances);
228 
229  const unsigned int number_of_indices = static_cast <unsigned int> (local_points.size ());
230  for (unsigned int i = 0; i < number_of_indices; i++)
231  local_triangles.insert (triangles_of_the_point_[local_points[i]].begin (), triangles_of_the_point_[local_points[i]].end ());
232 }
233 
234 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
235 template <typename PointInT, typename PointOutT> void
236 pcl::ROPSEstimation <PointInT, PointOutT>::computeLRF (const PointInT& point, const std::set <unsigned int>& local_triangles, Eigen::Matrix3f& lrf_matrix) const
237 {
238  const unsigned int number_of_triangles = static_cast <unsigned int> (local_triangles.size ());
239 
240  std::vector <Eigen::Matrix3f> scatter_matrices (number_of_triangles);
241  std::vector <float> triangle_area (number_of_triangles);
242  std::vector <float> distance_weight (number_of_triangles);
243 
244  float total_area = 0.0f;
245  const float coeff = 1.0f / 12.0f;
246  const float coeff_1_div_3 = 1.0f / 3.0f;
247 
248  Eigen::Vector3f feature_point (point.x, point.y, point.z);
249 
250  std::set <unsigned int>::const_iterator it;
251  unsigned int i_triangle = 0;
252  for (it = local_triangles.begin (), i_triangle = 0; it != local_triangles.end (); it++, i_triangle++)
253  {
254  Eigen::Vector3f pt[3];
255  for (unsigned int i_vertex = 0; i_vertex < 3; i_vertex++)
256  {
257  const unsigned int index = triangles_[*it].vertices[i_vertex];
258  pt[i_vertex] (0) = surface_->points[index].x;
259  pt[i_vertex] (1) = surface_->points[index].y;
260  pt[i_vertex] (2) = surface_->points[index].z;
261  }
262 
263  const float curr_area = ((pt[1] - pt[0]).cross (pt[2] - pt[0])).norm ();
264  triangle_area[i_triangle] = curr_area;
265  total_area += curr_area;
266 
267  distance_weight[i_triangle] = pow (support_radius_ - (feature_point - (pt[0] + pt[1] + pt[2]) * coeff_1_div_3).norm (), 2.0f);
268 
269  Eigen::Matrix3f curr_scatter_matrix;
270  curr_scatter_matrix.setZero ();
271  for (unsigned int i_pt = 0; i_pt < 3; i_pt++)
272  {
273  Eigen::Vector3f vec = pt[i_pt] - feature_point;
274  curr_scatter_matrix += vec * (vec.transpose ());
275  for (unsigned int j_pt = 0; j_pt < 3; j_pt++)
276  curr_scatter_matrix += vec * ((pt[j_pt] - feature_point).transpose ());
277  }
278  scatter_matrices[i_triangle] = coeff * curr_scatter_matrix;
279  }
280 
281  if (abs (total_area) < std::numeric_limits <float>::epsilon ())
282  total_area = 1.0f / total_area;
283  else
284  total_area = 1.0f;
285 
286  Eigen::Matrix3f overall_scatter_matrix;
287  overall_scatter_matrix.setZero ();
288  std::vector<float> total_weight (number_of_triangles);
289  const float denominator = 1.0f / 6.0f;
290  for (unsigned int i_triangle = 0; i_triangle < number_of_triangles; i_triangle++)
291  {
292  float factor = distance_weight[i_triangle] * triangle_area[i_triangle] * total_area;
293  overall_scatter_matrix += factor * scatter_matrices[i_triangle];
294  total_weight[i_triangle] = factor * denominator;
295  }
296 
297  Eigen::Vector3f v1, v2, v3;
298  computeEigenVectors (overall_scatter_matrix, v1, v2, v3);
299 
300  float h1 = 0.0f;
301  float h3 = 0.0f;
302  for (it = local_triangles.begin (), i_triangle = 0; it != local_triangles.end (); it++, i_triangle++)
303  {
304  Eigen::Vector3f pt[3];
305  for (unsigned int i_vertex = 0; i_vertex < 3; i_vertex++)
306  {
307  const unsigned int index = triangles_[*it].vertices[i_vertex];
308  pt[i_vertex] (0) = surface_->points[index].x;
309  pt[i_vertex] (1) = surface_->points[index].y;
310  pt[i_vertex] (2) = surface_->points[index].z;
311  }
312 
313  float factor1 = 0.0f;
314  float factor3 = 0.0f;
315  for (unsigned int i_pt = 0; i_pt < 3; i_pt++)
316  {
317  Eigen::Vector3f vec = pt[i_pt] - feature_point;
318  factor1 += vec.dot (v1);
319  factor3 += vec.dot (v3);
320  }
321  h1 += total_weight[i_triangle] * factor1;
322  h3 += total_weight[i_triangle] * factor3;
323  }
324 
325  if (h1 < 0.0f) v1 = -v1;
326  if (h3 < 0.0f) v3 = -v3;
327 
328  v2 = v3.cross (v1);
329 
330  lrf_matrix.row (0) = v1;
331  lrf_matrix.row (1) = v2;
332  lrf_matrix.row (2) = v3;
333 }
334 
335 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
336 template <typename PointInT, typename PointOutT> void
338  Eigen::Vector3f& major_axis, Eigen::Vector3f& middle_axis, Eigen::Vector3f& minor_axis) const
339 {
340  Eigen::EigenSolver <Eigen::Matrix3f> eigen_solver;
341  eigen_solver.compute (matrix);
342 
343  Eigen::EigenSolver <Eigen::Matrix3f>::EigenvectorsType eigen_vectors;
344  Eigen::EigenSolver <Eigen::Matrix3f>::EigenvalueType eigen_values;
345  eigen_vectors = eigen_solver.eigenvectors ();
346  eigen_values = eigen_solver.eigenvalues ();
347 
348  unsigned int temp = 0;
349  unsigned int major_index = 0;
350  unsigned int middle_index = 1;
351  unsigned int minor_index = 2;
352 
353  if (eigen_values.real () (major_index) < eigen_values.real () (middle_index))
354  {
355  temp = major_index;
356  major_index = middle_index;
357  middle_index = temp;
358  }
359 
360  if (eigen_values.real () (major_index) < eigen_values.real () (minor_index))
361  {
362  temp = major_index;
363  major_index = minor_index;
364  minor_index = temp;
365  }
366 
367  if (eigen_values.real () (middle_index) < eigen_values.real () (minor_index))
368  {
369  temp = minor_index;
370  minor_index = middle_index;
371  middle_index = temp;
372  }
373 
374  major_axis = eigen_vectors.col (major_index).real ();
375  middle_axis = eigen_vectors.col (middle_index).real ();
376  minor_axis = eigen_vectors.col (minor_index).real ();
377 }
378 
379 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
380 template <typename PointInT, typename PointOutT> void
381 pcl::ROPSEstimation <PointInT, PointOutT>::transformCloud (const PointInT& point, const Eigen::Matrix3f& matrix, const std::vector <int>& local_points, PointCloudIn& transformed_cloud) const
382 {
383  const unsigned int number_of_points = static_cast <unsigned int> (local_points.size ());
384  transformed_cloud.points.resize (number_of_points, PointInT ());
385 
386  for (unsigned int i = 0; i < number_of_points; i++)
387  {
388  Eigen::Vector3f transformed_point (
389  surface_->points[local_points[i]].x - point.x,
390  surface_->points[local_points[i]].y - point.y,
391  surface_->points[local_points[i]].z - point.z);
392 
393  transformed_point = matrix * transformed_point;
394 
395  PointInT new_point;
396  new_point.x = transformed_point (0);
397  new_point.y = transformed_point (1);
398  new_point.z = transformed_point (2);
399  transformed_cloud.points[i] = new_point;
400  }
401 }
402 
403 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
404 template <typename PointInT, typename PointOutT> void
405 pcl::ROPSEstimation <PointInT, PointOutT>::rotateCloud (const PointInT& axis, const float angle, const PointCloudIn& cloud, PointCloudIn& rotated_cloud, Eigen::Vector3f& min, Eigen::Vector3f& max) const
406 {
407  Eigen::Matrix3f rotation_matrix;
408  const float x = axis.x;
409  const float y = axis.y;
410  const float z = axis.z;
411  const float rad = M_PI / 180.0f;
412  const float cosine = cos (angle * rad);
413  const float sine = sin (angle * rad);
414  rotation_matrix << cosine + (1 - cosine) * x * x, (1 - cosine) * x * y - sine * z, (1 - cosine) * x * z + sine * y,
415  (1 - cosine) * y * x + sine * z, cosine + (1 - cosine) * y * y, (1 - cosine) * y * z - sine * x,
416  (1 - cosine) * z * x - sine * y, (1 - cosine) * z * y + sine * x, cosine + (1 - cosine) * z * z;
417 
418  const unsigned int number_of_points = static_cast <unsigned int> (cloud.points.size ());
419 
420  rotated_cloud.header = cloud.header;
421  rotated_cloud.width = number_of_points;
422  rotated_cloud.height = 1;
423  rotated_cloud.points.resize (number_of_points, PointInT ());
424 
425  min (0) = std::numeric_limits <float>::max ();
426  min (1) = std::numeric_limits <float>::max ();
427  min (2) = std::numeric_limits <float>::max ();
428  max (0) = -std::numeric_limits <float>::max ();
429  max (1) = -std::numeric_limits <float>::max ();
430  max (2) = -std::numeric_limits <float>::max ();
431 
432  for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
433  {
434  Eigen::Vector3f point (
435  cloud.points[i_point].x,
436  cloud.points[i_point].y,
437  cloud.points[i_point].z);
438 
439  point = rotation_matrix * point;
440  PointInT rotated_point;
441  rotated_point.x = point (0);
442  rotated_point.y = point (1);
443  rotated_point.z = point (2);
444  rotated_cloud.points[i_point] = rotated_point;
445 
446  if (min (0) > rotated_point.x) min (0) = rotated_point.x;
447  if (min (1) > rotated_point.y) min (1) = rotated_point.y;
448  if (min (2) > rotated_point.z) min (2) = rotated_point.z;
449 
450  if (max (0) < rotated_point.x) max (0) = rotated_point.x;
451  if (max (1) < rotated_point.y) max (1) = rotated_point.y;
452  if (max (2) < rotated_point.z) max (2) = rotated_point.z;
453  }
454 }
455 
456 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
457 template <typename PointInT, typename PointOutT> void
458 pcl::ROPSEstimation <PointInT, PointOutT>::getDistributionMatrix (const unsigned int projection, const Eigen::Vector3f& min, const Eigen::Vector3f& max, const PointCloudIn& cloud, Eigen::MatrixXf& matrix) const
459 {
460  matrix.setZero ();
461 
462  const unsigned int number_of_points = static_cast <unsigned int> (cloud.points.size ());
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 (unsigned int i_point = 0; i_point < number_of_points; i_point++)
473  {
474  Eigen::Vector3f point (
475  cloud.points[i_point].x,
476  cloud.points[i_point].y,
477  cloud.points[i_point].z);
478 
479  const float u_length = point (coord[projection][0]) - min[coord[projection][0]];
480  const float v_length = point (coord[projection][1]) - min[coord[projection][1]];
481 
482  const float u_ratio = u_length / u_bin_length;
483  unsigned int row = static_cast <unsigned int> (u_ratio);
484  if (row == number_of_bins_) row--;
485 
486  const float v_ratio = v_length / v_bin_length;
487  unsigned int col = static_cast <unsigned int> (v_ratio);
488  if (col == number_of_bins_) col--;
489 
490  matrix (row, col) += 1.0f;
491  }
492 
493  matrix /= static_cast <float> (number_of_points);
494 }
495 
496 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
497 template <typename PointInT, typename PointOutT> void
498 pcl::ROPSEstimation <PointInT, PointOutT>::computeCentralMoments (const Eigen::MatrixXf& matrix, std::vector <float>& moments) const
499 {
500  float mean_i = 0.0f;
501  float mean_j = 0.0f;
502 
503  for (unsigned int i = 0; i < number_of_bins_; i++)
504  for (unsigned int j = 0; j < number_of_bins_; j++)
505  {
506  const float m = matrix (i, j);
507  mean_i += static_cast <float> (i + 1) * m;
508  mean_j += static_cast <float> (j + 1) * m;
509  }
510 
511  const unsigned int number_of_moments_to_compute = 4;
512  const float power[number_of_moments_to_compute][2] = {
513  {1.0f, 1.0f},
514  {2.0f, 1.0f},
515  {1.0f, 2.0f},
516  {2.0f, 2.0f}};
517 
518  float entropy = 0.0f;
519  moments.resize (number_of_moments_to_compute + 1, 0.0f);
520  for (unsigned int i = 0; i < number_of_bins_; i++)
521  {
522  const float i_factor = static_cast <float> (i + 1) - mean_i;
523  for (unsigned int j = 0; j < number_of_bins_; j++)
524  {
525  const float j_factor = static_cast <float> (j + 1) - mean_j;
526  const float m = matrix (i, j);
527  if (m > 0.0f)
528  entropy -= m * log (m);
529  for (unsigned int i_moment = 0; i_moment < number_of_moments_to_compute; i_moment++)
530  moments[i_moment] += pow (i_factor, power[i_moment][0]) * pow (j_factor, power[i_moment][1]) * m;
531  }
532  }
533 
534  moments[number_of_moments_to_compute] = entropy;
535 }
536 
537 #endif // PCL_ROPS_ESTIMATION_HPP_
This class implements the method for extracting RoPS features presented in the article "Rotational Pr...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition: feature.hpp:189