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