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...
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in using th...
Definition: feature.hpp:189