Point Cloud Library (PCL)  1.12.1-dev
moment_of_inertia_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_MOMENT_OF_INERTIA_ESTIMATION_HPP_
41 #define PCL_MOMENT_OF_INERTIA_ESTIMATION_HPP_
42 
43 #include <Eigen/Eigenvalues> // for EigenSolver
44 
45 #include <pcl/features/moment_of_inertia_estimation.h>
46 #include <pcl/features/feature.h>
47 
48 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
49 template <typename PointT>
51  is_valid_ (false),
52  step_ (10.0f),
53  point_mass_ (0.0001f),
54  normalize_ (true),
55  mean_value_ (0.0f, 0.0f, 0.0f),
56  major_axis_ (0.0f, 0.0f, 0.0f),
57  middle_axis_ (0.0f, 0.0f, 0.0f),
58  minor_axis_ (0.0f, 0.0f, 0.0f),
59  major_value_ (0.0f),
60  middle_value_ (0.0f),
61  minor_value_ (0.0f),
62  aabb_min_point_ (),
63  aabb_max_point_ (),
64  obb_min_point_ (),
65  obb_max_point_ (),
66  obb_position_ (0.0f, 0.0f, 0.0f)
67 {
68 }
69 
70 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
71 template <typename PointT>
73 {
74  moment_of_inertia_.clear ();
75  eccentricity_.clear ();
76 }
77 
78 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
79 template <typename PointT> void
81 {
82  if (step <= 0.0f)
83  return;
84 
85  step_ = step;
86 
87  is_valid_ = false;
88 }
89 
90 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
91 template <typename PointT> float
93 {
94  return (step_);
95 }
96 
97 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
98 template <typename PointT> void
100 {
101  normalize_ = need_to_normalize;
102 
103  is_valid_ = false;
104 }
105 
106 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
107 template <typename PointT> bool
109 {
110  return (normalize_);
111 }
112 
113 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
114 template <typename PointT> void
116 {
117  if (point_mass <= 0.0f)
118  return;
119 
120  point_mass_ = point_mass;
121 
122  is_valid_ = false;
123 }
124 
125 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
126 template <typename PointT> float
128 {
129  return (point_mass_);
130 }
131 
132 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
133 template <typename PointT> void
135 {
136  moment_of_inertia_.clear ();
137  eccentricity_.clear ();
138 
139  if (!initCompute ())
140  {
141  deinitCompute ();
142  return;
143  }
144 
145  if (normalize_)
146  {
147  if (!indices_->empty ())
148  point_mass_ = 1.0f / static_cast <float> (indices_->size () * indices_->size ());
149  else
150  point_mass_ = 1.0f;
151  }
152 
153  computeMeanValue ();
154 
155  Eigen::Matrix <float, 3, 3> covariance_matrix;
156  covariance_matrix.setZero ();
157  computeCovarianceMatrix (covariance_matrix);
158 
159  computeEigenVectors (covariance_matrix, major_axis_, middle_axis_, minor_axis_, major_value_, middle_value_, minor_value_);
160 
161  float theta = 0.0f;
162  while (theta <= 90.0f)
163  {
164  float phi = 0.0f;
165  Eigen::Vector3f rotated_vector;
166  rotateVector (major_axis_, middle_axis_, theta, rotated_vector);
167  while (phi <= 360.0f)
168  {
169  Eigen::Vector3f current_axis;
170  rotateVector (rotated_vector, minor_axis_, phi, current_axis);
171  current_axis.normalize ();
172 
173  //compute moment of inertia for the current axis
174  float current_moment_of_inertia = calculateMomentOfInertia (current_axis, mean_value_);
175  moment_of_inertia_.push_back (current_moment_of_inertia);
176 
177  //compute eccentricity for the current plane
178  typename pcl::PointCloud<PointT>::Ptr projected_cloud (new pcl::PointCloud<PointT> ());
179  getProjectedCloud (current_axis, mean_value_, projected_cloud);
180  Eigen::Matrix <float, 3, 3> covariance_matrix;
181  covariance_matrix.setZero ();
182  computeCovarianceMatrix (projected_cloud, covariance_matrix);
183  projected_cloud.reset ();
184  float current_eccentricity = computeEccentricity (covariance_matrix, current_axis);
185  eccentricity_.push_back (current_eccentricity);
186 
187  phi += step_;
188  }
189  theta += step_;
190  }
191 
192  computeOBB ();
193 
194  is_valid_ = true;
195 
196  deinitCompute ();
197 }
198 
199 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
200 template <typename PointT> bool
202 {
203  min_point = aabb_min_point_;
204  max_point = aabb_max_point_;
205 
206  return (is_valid_);
207 }
208 
209 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
210 template <typename PointT> bool
211 pcl::MomentOfInertiaEstimation<PointT>::getOBB (PointT& min_point, PointT& max_point, PointT& position, Eigen::Matrix3f& rotational_matrix) const
212 {
213  min_point = obb_min_point_;
214  max_point = obb_max_point_;
215  position.x = obb_position_ (0);
216  position.y = obb_position_ (1);
217  position.z = obb_position_ (2);
218  rotational_matrix = obb_rotational_matrix_;
219 
220  return (is_valid_);
221 }
222 
223 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
224 template <typename PointT> void
226 {
227  obb_min_point_.x = std::numeric_limits <float>::max ();
228  obb_min_point_.y = std::numeric_limits <float>::max ();
229  obb_min_point_.z = std::numeric_limits <float>::max ();
230 
231  obb_max_point_.x = std::numeric_limits <float>::min ();
232  obb_max_point_.y = std::numeric_limits <float>::min ();
233  obb_max_point_.z = std::numeric_limits <float>::min ();
234 
235  unsigned int number_of_points = static_cast <unsigned int> (indices_->size ());
236  for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
237  {
238  float x = ((*input_)[(*indices_)[i_point]].x - mean_value_ (0)) * major_axis_ (0) +
239  ((*input_)[(*indices_)[i_point]].y - mean_value_ (1)) * major_axis_ (1) +
240  ((*input_)[(*indices_)[i_point]].z - mean_value_ (2)) * major_axis_ (2);
241  float y = ((*input_)[(*indices_)[i_point]].x - mean_value_ (0)) * middle_axis_ (0) +
242  ((*input_)[(*indices_)[i_point]].y - mean_value_ (1)) * middle_axis_ (1) +
243  ((*input_)[(*indices_)[i_point]].z - mean_value_ (2)) * middle_axis_ (2);
244  float z = ((*input_)[(*indices_)[i_point]].x - mean_value_ (0)) * minor_axis_ (0) +
245  ((*input_)[(*indices_)[i_point]].y - mean_value_ (1)) * minor_axis_ (1) +
246  ((*input_)[(*indices_)[i_point]].z - mean_value_ (2)) * minor_axis_ (2);
247 
248  if (x <= obb_min_point_.x) obb_min_point_.x = x;
249  if (y <= obb_min_point_.y) obb_min_point_.y = y;
250  if (z <= obb_min_point_.z) obb_min_point_.z = z;
251 
252  if (x >= obb_max_point_.x) obb_max_point_.x = x;
253  if (y >= obb_max_point_.y) obb_max_point_.y = y;
254  if (z >= obb_max_point_.z) obb_max_point_.z = z;
255  }
256 
257  obb_rotational_matrix_ << major_axis_ (0), middle_axis_ (0), minor_axis_ (0),
258  major_axis_ (1), middle_axis_ (1), minor_axis_ (1),
259  major_axis_ (2), middle_axis_ (2), minor_axis_ (2);
260 
261  Eigen::Vector3f shift (
262  (obb_max_point_.x + obb_min_point_.x) / 2.0f,
263  (obb_max_point_.y + obb_min_point_.y) / 2.0f,
264  (obb_max_point_.z + obb_min_point_.z) / 2.0f);
265 
266  obb_min_point_.x -= shift (0);
267  obb_min_point_.y -= shift (1);
268  obb_min_point_.z -= shift (2);
269 
270  obb_max_point_.x -= shift (0);
271  obb_max_point_.y -= shift (1);
272  obb_max_point_.z -= shift (2);
273 
274  obb_position_ = mean_value_ + obb_rotational_matrix_ * shift;
275 }
276 
277 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
278 template <typename PointT> bool
279 pcl::MomentOfInertiaEstimation<PointT>::getEigenValues (float& major, float& middle, float& minor) const
280 {
281  major = major_value_;
282  middle = middle_value_;
283  minor = minor_value_;
284 
285  return (is_valid_);
286 }
287 
288 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
289 template <typename PointT> bool
290 pcl::MomentOfInertiaEstimation<PointT>::getEigenVectors (Eigen::Vector3f& major, Eigen::Vector3f& middle, Eigen::Vector3f& minor) const
291 {
292  major = major_axis_;
293  middle = middle_axis_;
294  minor = minor_axis_;
295 
296  return (is_valid_);
297 }
298 
299 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
300 template <typename PointT> bool
301 pcl::MomentOfInertiaEstimation<PointT>::getMomentOfInertia (std::vector <float>& moment_of_inertia) const
302 {
303  moment_of_inertia.resize (moment_of_inertia_.size (), 0.0f);
304  std::copy (moment_of_inertia_.cbegin (), moment_of_inertia_.cend (), moment_of_inertia.begin ());
305 
306  return (is_valid_);
307 }
308 
309 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
310 template <typename PointT> bool
311 pcl::MomentOfInertiaEstimation<PointT>::getEccentricity (std::vector <float>& eccentricity) const
312 {
313  eccentricity.resize (eccentricity_.size (), 0.0f);
314  std::copy (eccentricity_.cbegin (), eccentricity_.cend (), eccentricity.begin ());
315 
316  return (is_valid_);
317 }
318 
319 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
320 template <typename PointT> void
322 {
323  mean_value_ (0) = 0.0f;
324  mean_value_ (1) = 0.0f;
325  mean_value_ (2) = 0.0f;
326 
327  aabb_min_point_.x = std::numeric_limits <float>::max ();
328  aabb_min_point_.y = std::numeric_limits <float>::max ();
329  aabb_min_point_.z = std::numeric_limits <float>::max ();
330 
331  aabb_max_point_.x = -std::numeric_limits <float>::max ();
332  aabb_max_point_.y = -std::numeric_limits <float>::max ();
333  aabb_max_point_.z = -std::numeric_limits <float>::max ();
334 
335  unsigned int number_of_points = static_cast <unsigned int> (indices_->size ());
336  for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
337  {
338  mean_value_ (0) += (*input_)[(*indices_)[i_point]].x;
339  mean_value_ (1) += (*input_)[(*indices_)[i_point]].y;
340  mean_value_ (2) += (*input_)[(*indices_)[i_point]].z;
341 
342  if ((*input_)[(*indices_)[i_point]].x <= aabb_min_point_.x) aabb_min_point_.x = (*input_)[(*indices_)[i_point]].x;
343  if ((*input_)[(*indices_)[i_point]].y <= aabb_min_point_.y) aabb_min_point_.y = (*input_)[(*indices_)[i_point]].y;
344  if ((*input_)[(*indices_)[i_point]].z <= aabb_min_point_.z) aabb_min_point_.z = (*input_)[(*indices_)[i_point]].z;
345 
346  if ((*input_)[(*indices_)[i_point]].x >= aabb_max_point_.x) aabb_max_point_.x = (*input_)[(*indices_)[i_point]].x;
347  if ((*input_)[(*indices_)[i_point]].y >= aabb_max_point_.y) aabb_max_point_.y = (*input_)[(*indices_)[i_point]].y;
348  if ((*input_)[(*indices_)[i_point]].z >= aabb_max_point_.z) aabb_max_point_.z = (*input_)[(*indices_)[i_point]].z;
349  }
350 
351  if (number_of_points == 0)
352  number_of_points = 1;
353 
354  mean_value_ (0) /= number_of_points;
355  mean_value_ (1) /= number_of_points;
356  mean_value_ (2) /= number_of_points;
357 }
358 
359 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
360 template <typename PointT> void
361 pcl::MomentOfInertiaEstimation<PointT>::computeCovarianceMatrix (Eigen::Matrix <float, 3, 3>& covariance_matrix) const
362 {
363  covariance_matrix.setZero ();
364 
365  unsigned int number_of_points = static_cast <unsigned int> (indices_->size ());
366  float factor = 1.0f / static_cast <float> ((number_of_points - 1 > 0)?(number_of_points - 1):1);
367  for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
368  {
369  Eigen::Vector3f current_point (0.0f, 0.0f, 0.0f);
370  current_point (0) = (*input_)[(*indices_)[i_point]].x - mean_value_ (0);
371  current_point (1) = (*input_)[(*indices_)[i_point]].y - mean_value_ (1);
372  current_point (2) = (*input_)[(*indices_)[i_point]].z - mean_value_ (2);
373 
374  covariance_matrix += current_point * current_point.transpose ();
375  }
376 
377  covariance_matrix *= factor;
378 }
379 
380 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
381 template <typename PointT> void
382 pcl::MomentOfInertiaEstimation<PointT>::computeCovarianceMatrix (PointCloudConstPtr cloud, Eigen::Matrix <float, 3, 3>& covariance_matrix) const
383 {
384  covariance_matrix.setZero ();
385 
386  const auto number_of_points = cloud->size ();
387  float factor = 1.0f / static_cast <float> ((number_of_points - 1 > 0)?(number_of_points - 1):1);
388  Eigen::Vector3f current_point;
389  for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
390  {
391  current_point (0) = (*cloud)[i_point].x - mean_value_ (0);
392  current_point (1) = (*cloud)[i_point].y - mean_value_ (1);
393  current_point (2) = (*cloud)[i_point].z - mean_value_ (2);
394 
395  covariance_matrix += current_point * current_point.transpose ();
396  }
397 
398  covariance_matrix *= factor;
399 }
400 
401 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
402 template <typename PointT> void
403 pcl::MomentOfInertiaEstimation<PointT>::computeEigenVectors (const Eigen::Matrix <float, 3, 3>& covariance_matrix,
404  Eigen::Vector3f& major_axis, Eigen::Vector3f& middle_axis, Eigen::Vector3f& minor_axis, float& major_value,
405  float& middle_value, float& minor_value)
406 {
407  Eigen::EigenSolver <Eigen::Matrix <float, 3, 3> > eigen_solver;
408  eigen_solver.compute (covariance_matrix);
409 
410  Eigen::EigenSolver <Eigen::Matrix <float, 3, 3> >::EigenvectorsType eigen_vectors;
411  Eigen::EigenSolver <Eigen::Matrix <float, 3, 3> >::EigenvalueType eigen_values;
412  eigen_vectors = eigen_solver.eigenvectors ();
413  eigen_values = eigen_solver.eigenvalues ();
414 
415  unsigned int temp = 0;
416  unsigned int major_index = 0;
417  unsigned int middle_index = 1;
418  unsigned int minor_index = 2;
419 
420  if (eigen_values.real () (major_index) < eigen_values.real () (middle_index))
421  {
422  temp = major_index;
423  major_index = middle_index;
424  middle_index = temp;
425  }
426 
427  if (eigen_values.real () (major_index) < eigen_values.real () (minor_index))
428  {
429  temp = major_index;
430  major_index = minor_index;
431  minor_index = temp;
432  }
433 
434  if (eigen_values.real () (middle_index) < eigen_values.real () (minor_index))
435  {
436  temp = minor_index;
437  minor_index = middle_index;
438  middle_index = temp;
439  }
440 
441  major_value = eigen_values.real () (major_index);
442  middle_value = eigen_values.real () (middle_index);
443  minor_value = eigen_values.real () (minor_index);
444 
445  major_axis = eigen_vectors.col (major_index).real ();
446  middle_axis = eigen_vectors.col (middle_index).real ();
447  minor_axis = eigen_vectors.col (minor_index).real ();
448 
449  major_axis.normalize ();
450  middle_axis.normalize ();
451  minor_axis.normalize ();
452 
453  float det = major_axis.dot (middle_axis.cross (minor_axis));
454  if (det <= 0.0f)
455  {
456  major_axis (0) = -major_axis (0);
457  major_axis (1) = -major_axis (1);
458  major_axis (2) = -major_axis (2);
459  }
460 }
461 
462 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
463 template <typename PointT> void
464 pcl::MomentOfInertiaEstimation<PointT>::rotateVector (const Eigen::Vector3f& vector, const Eigen::Vector3f& axis, const float angle, Eigen::Vector3f& rotated_vector) const
465 {
466  Eigen::Matrix <float, 3, 3> rotation_matrix;
467  const float x = axis (0);
468  const float y = axis (1);
469  const float z = axis (2);
470  const float rad = M_PI / 180.0f;
471  const float cosine = std::cos (angle * rad);
472  const float sine = std::sin (angle * rad);
473  rotation_matrix << cosine + (1 - cosine) * x * x, (1 - cosine) * x * y - sine * z, (1 - cosine) * x * z + sine * y,
474  (1 - cosine) * y * x + sine * z, cosine + (1 - cosine) * y * y, (1 - cosine) * y * z - sine * x,
475  (1 - cosine) * z * x - sine * y, (1 - cosine) * z * y + sine * x, cosine + (1 - cosine) * z * z;
476 
477  rotated_vector = rotation_matrix * vector;
478 }
479 
480 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
481 template <typename PointT> float
482 pcl::MomentOfInertiaEstimation<PointT>::calculateMomentOfInertia (const Eigen::Vector3f& current_axis, const Eigen::Vector3f& mean_value) const
483 {
484  float moment_of_inertia = 0.0f;
485  unsigned int number_of_points = static_cast <unsigned int> (indices_->size ());
486  for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
487  {
488  Eigen::Vector3f vector;
489  vector (0) = mean_value (0) - (*input_)[(*indices_)[i_point]].x;
490  vector (1) = mean_value (1) - (*input_)[(*indices_)[i_point]].y;
491  vector (2) = mean_value (2) - (*input_)[(*indices_)[i_point]].z;
492 
493  Eigen::Vector3f product = vector.cross (current_axis);
494 
495  float distance = product (0) * product (0) + product (1) * product (1) + product (2) * product (2);
496 
497  moment_of_inertia += distance;
498  }
499 
500  return (point_mass_ * moment_of_inertia);
501 }
502 
503 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
504 template <typename PointT> void
505 pcl::MomentOfInertiaEstimation<PointT>::getProjectedCloud (const Eigen::Vector3f& normal_vector, const Eigen::Vector3f& point, typename pcl::PointCloud <PointT>::Ptr projected_cloud) const
506 {
507  const float D = - normal_vector.dot (point);
508 
509  unsigned int number_of_points = static_cast <unsigned int> (indices_->size ());
510  projected_cloud->points.resize (number_of_points, PointT ());
511 
512  for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
513  {
514  const unsigned int index = (*indices_)[i_point];
515  float K = - (D + normal_vector (0) * (*input_)[index].x + normal_vector (1) * (*input_)[index].y + normal_vector (2) * (*input_)[index].z);
516  PointT projected_point;
517  projected_point.x = (*input_)[index].x + K * normal_vector (0);
518  projected_point.y = (*input_)[index].y + K * normal_vector (1);
519  projected_point.z = (*input_)[index].z + K * normal_vector (2);
520  (*projected_cloud)[i_point] = projected_point;
521  }
522  projected_cloud->width = number_of_points;
523  projected_cloud->height = 1;
524  projected_cloud->header = input_->header;
525 }
526 
527 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
528 template <typename PointT> float
529 pcl::MomentOfInertiaEstimation<PointT>::computeEccentricity (const Eigen::Matrix <float, 3, 3>& covariance_matrix, const Eigen::Vector3f& normal_vector)
530 {
531  Eigen::Vector3f major_axis (0.0f, 0.0f, 0.0f);
532  Eigen::Vector3f middle_axis (0.0f, 0.0f, 0.0f);
533  Eigen::Vector3f minor_axis (0.0f, 0.0f, 0.0f);
534  float major_value = 0.0f;
535  float middle_value = 0.0f;
536  float minor_value = 0.0f;
537  computeEigenVectors (covariance_matrix, major_axis, middle_axis, minor_axis, major_value, middle_value, minor_value);
538 
539  float major = std::abs (major_axis.dot (normal_vector));
540  float middle = std::abs (middle_axis.dot (normal_vector));
541  float minor = std::abs (minor_axis.dot (normal_vector));
542 
543  float eccentricity = 0.0f;
544 
545  if (major >= middle && major >= minor && middle_value != 0.0f)
546  eccentricity = std::pow (1.0f - (minor_value * minor_value) / (middle_value * middle_value), 0.5f);
547 
548  if (middle >= major && middle >= minor && major_value != 0.0f)
549  eccentricity = std::pow (1.0f - (minor_value * minor_value) / (major_value * major_value), 0.5f);
550 
551  if (minor >= major && minor >= middle && major_value != 0.0f)
552  eccentricity = std::pow (1.0f - (middle_value * middle_value) / (major_value * major_value), 0.5f);
553 
554  return (eccentricity);
555 }
556 
557 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
558 template <typename PointT> bool
559 pcl::MomentOfInertiaEstimation<PointT>::getMassCenter (Eigen::Vector3f& mass_center) const
560 {
561  mass_center = mean_value_;
562 
563  return (is_valid_);
564 }
565 
566 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
567 template <typename PointT> void
569 {
571  is_valid_ = false;
572 }
573 
574 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
575 template <typename PointT> void
577 {
579  is_valid_ = false;
580 }
581 
582 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
583 template <typename PointT> void
585 {
587  is_valid_ = false;
588 }
589 
590 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
591 template <typename PointT> void
593 {
595  is_valid_ = false;
596 }
597 
598 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
599 template <typename PointT> void
600 pcl::MomentOfInertiaEstimation<PointT>::setIndices (std::size_t row_start, std::size_t col_start, std::size_t nb_rows, std::size_t nb_cols)
601 {
602  pcl::PCLBase<PointT>::setIndices (row_start, col_start, nb_rows, nb_cols);
603  is_valid_ = false;
604 }
605 
606 #endif // PCL_MOMENT_OF_INERTIA_ESTIMATION_HPP_
Implements the method for extracting features based on moment of inertia.
float getAngleStep() const
Returns the angle step.
~MomentOfInertiaEstimation() override
Virtual destructor which frees the memory.
void setIndices(const IndicesPtr &indices) override
Provide a pointer to the vector of indices that represents the input data.
void compute()
This method launches the computation of all features.
bool getEigenValues(float &major, float &middle, float &minor) const
This method gives access to the computed eigen values.
float getPointMass() const
Returns the mass of point.
bool getEccentricity(std::vector< float > &eccentricity) const
This method gives access to the computed ecentricities.
bool getOBB(PointT &min_point, PointT &max_point, PointT &position, Eigen::Matrix3f &rotational_matrix) const
This method gives access to the computed oriented bounding box.
bool getNormalizePointMassFlag() const
Returns the normalize_ flag.
bool getAABB(PointT &min_point, PointT &max_point) const
This method gives access to the computed axis aligned bounding box.
void setNormalizePointMassFlag(bool need_to_normalize)
This method allows to set the normalize_ flag.
void setPointMass(const float point_mass)
This method allows to set point mass that will be used for moment of inertia calculation.
void setAngleStep(const float step)
This method allows to set the angle step.
MomentOfInertiaEstimation()
Constructor that sets default values for member variables.
bool getMassCenter(Eigen::Vector3f &mass_center) const
This method gives access to the computed mass center.
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
bool getMomentOfInertia(std::vector< float > &moment_of_inertia) const
This method gives access to the computed moments of inertia.
bool getEigenVectors(Eigen::Vector3f &major, Eigen::Vector3f &middle, Eigen::Vector3f &minor) const
This method gives access to the computed eigen vectors.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:65
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: pcl_base.h:74
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
Definition: pcl_base.hpp:72
PointIndices::ConstPtr PointIndicesConstPtr
Definition: pcl_base.h:77
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:392
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:395
unsigned int computeCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, const Eigen::Matrix< Scalar, 4, 1 > &centroid, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix)
Compute the 3x3 covariance matrix of a given set of points.
Definition: centroid.hpp:191
@ K
Definition: norms.h:54
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
shared_ptr< const Indices > IndicesConstPtr
Definition: pcl_base.h:59
shared_ptr< Indices > IndicesPtr
Definition: pcl_base.h:58
#define M_PI
Definition: pcl_macros.h:201
A point structure representing Euclidean xyz coordinates, and the RGB color.