Point Cloud Library (PCL)  1.15.0-dev
All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Modules Pages
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  const Eigen::SelfAdjointEigenSolver<Eigen::Matrix<float, 3, 3>> eigen_solver(covariance_matrix);
403 
404  const Eigen::SelfAdjointEigenSolver <Eigen::Matrix <float, 3, 3> >::EigenvectorsType& eigen_vectors = eigen_solver.eigenvectors ();
405  const Eigen::SelfAdjointEigenSolver <Eigen::Matrix <float, 3, 3> >::RealVectorType& eigen_values = eigen_solver.eigenvalues ();
406 
407  unsigned int temp = 0;
408  unsigned int major_index = 0;
409  unsigned int middle_index = 1;
410  unsigned int minor_index = 2;
411 
412  if (eigen_values (major_index) < eigen_values (middle_index))
413  {
414  temp = major_index;
415  major_index = middle_index;
416  middle_index = temp;
417  }
418 
419  if (eigen_values (major_index) < eigen_values (minor_index))
420  {
421  temp = major_index;
422  major_index = minor_index;
423  minor_index = temp;
424  }
425 
426  if (eigen_values (middle_index) < eigen_values (minor_index))
427  {
428  temp = minor_index;
429  minor_index = middle_index;
430  middle_index = temp;
431  }
432 
433  major_value = eigen_values (major_index);
434  middle_value = eigen_values (middle_index);
435  minor_value = eigen_values (minor_index);
436 
437  major_axis = eigen_vectors.col (major_index);
438  middle_axis = eigen_vectors.col (middle_index);
439  minor_axis = eigen_vectors.col (minor_index);
440 
441  major_axis.normalize ();
442  middle_axis.normalize ();
443  minor_axis.normalize ();
444 
445  float det = major_axis.dot (middle_axis.cross (minor_axis));
446  if (det <= 0.0f)
447  {
448  major_axis (0) = -major_axis (0);
449  major_axis (1) = -major_axis (1);
450  major_axis (2) = -major_axis (2);
451  }
452 }
453 
454 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
455 template <typename PointT> void
456 pcl::MomentOfInertiaEstimation<PointT>::rotateVector (const Eigen::Vector3f& vector, const Eigen::Vector3f& axis, const float angle, Eigen::Vector3f& rotated_vector) const
457 {
458  Eigen::Matrix <float, 3, 3> rotation_matrix;
459  const float x = axis (0);
460  const float y = axis (1);
461  const float z = axis (2);
462  const float rad = M_PI / 180.0f;
463  const float cosine = std::cos (angle * rad);
464  const float sine = std::sin (angle * rad);
465  rotation_matrix << cosine + (1 - cosine) * x * x, (1 - cosine) * x * y - sine * z, (1 - cosine) * x * z + sine * y,
466  (1 - cosine) * y * x + sine * z, cosine + (1 - cosine) * y * y, (1 - cosine) * y * z - sine * x,
467  (1 - cosine) * z * x - sine * y, (1 - cosine) * z * y + sine * x, cosine + (1 - cosine) * z * z;
468 
469  rotated_vector = rotation_matrix * vector;
470 }
471 
472 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
473 template <typename PointT> float
474 pcl::MomentOfInertiaEstimation<PointT>::calculateMomentOfInertia (const Eigen::Vector3f& current_axis, const Eigen::Vector3f& mean_value) const
475 {
476  float moment_of_inertia = 0.0f;
477  auto number_of_points = static_cast <unsigned int> (indices_->size ());
478  for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
479  {
480  Eigen::Vector3f vector;
481  vector (0) = mean_value (0) - (*input_)[(*indices_)[i_point]].x;
482  vector (1) = mean_value (1) - (*input_)[(*indices_)[i_point]].y;
483  vector (2) = mean_value (2) - (*input_)[(*indices_)[i_point]].z;
484 
485  Eigen::Vector3f product = vector.cross (current_axis);
486 
487  float distance = product (0) * product (0) + product (1) * product (1) + product (2) * product (2);
488 
489  moment_of_inertia += distance;
490  }
491 
492  return (point_mass_ * moment_of_inertia);
493 }
494 
495 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
496 template <typename PointT> void
497 pcl::MomentOfInertiaEstimation<PointT>::getProjectedCloud (const Eigen::Vector3f& normal_vector, const Eigen::Vector3f& point, typename pcl::PointCloud <PointT>::Ptr projected_cloud) const
498 {
499  const float D = - normal_vector.dot (point);
500 
501  auto number_of_points = static_cast <unsigned int> (indices_->size ());
502  projected_cloud->points.resize (number_of_points, PointT ());
503 
504  for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
505  {
506  const unsigned int index = (*indices_)[i_point];
507  float K = - (D + normal_vector (0) * (*input_)[index].x + normal_vector (1) * (*input_)[index].y + normal_vector (2) * (*input_)[index].z);
508  PointT projected_point;
509  projected_point.x = (*input_)[index].x + K * normal_vector (0);
510  projected_point.y = (*input_)[index].y + K * normal_vector (1);
511  projected_point.z = (*input_)[index].z + K * normal_vector (2);
512  (*projected_cloud)[i_point] = projected_point;
513  }
514  projected_cloud->width = number_of_points;
515  projected_cloud->height = 1;
516  projected_cloud->header = input_->header;
517 }
518 
519 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
520 template <typename PointT> float
521 pcl::MomentOfInertiaEstimation<PointT>::computeEccentricity (const Eigen::Matrix <float, 3, 3>& covariance_matrix, const Eigen::Vector3f& normal_vector)
522 {
523  Eigen::Vector3f major_axis (0.0f, 0.0f, 0.0f);
524  Eigen::Vector3f middle_axis (0.0f, 0.0f, 0.0f);
525  Eigen::Vector3f minor_axis (0.0f, 0.0f, 0.0f);
526  float major_value = 0.0f;
527  float middle_value = 0.0f;
528  float minor_value = 0.0f;
529  computeEigenVectors (covariance_matrix, major_axis, middle_axis, minor_axis, major_value, middle_value, minor_value);
530 
531  float major = std::abs (major_axis.dot (normal_vector));
532  float middle = std::abs (middle_axis.dot (normal_vector));
533  float minor = std::abs (minor_axis.dot (normal_vector));
534 
535  float eccentricity = 0.0f;
536 
537  if (major >= middle && major >= minor && middle_value != 0.0f)
538  eccentricity = std::pow (1.0f - (minor_value * minor_value) / (middle_value * middle_value), 0.5f);
539 
540  if (middle >= major && middle >= minor && major_value != 0.0f)
541  eccentricity = std::pow (1.0f - (minor_value * minor_value) / (major_value * major_value), 0.5f);
542 
543  if (minor >= major && minor >= middle && major_value != 0.0f)
544  eccentricity = std::pow (1.0f - (middle_value * middle_value) / (major_value * major_value), 0.5f);
545 
546  return (eccentricity);
547 }
548 
549 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
550 template <typename PointT> bool
551 pcl::MomentOfInertiaEstimation<PointT>::getMassCenter (Eigen::Vector3f& mass_center) const
552 {
553  mass_center = mean_value_;
554 
555  return (is_valid_);
556 }
557 
558 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
559 template <typename PointT> void
561 {
563  is_valid_ = false;
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
592 pcl::MomentOfInertiaEstimation<PointT>::setIndices (std::size_t row_start, std::size_t col_start, std::size_t nb_rows, std::size_t nb_cols)
593 {
594  pcl::PCLBase<PointT>::setIndices (row_start, col_start, nb_rows, nb_cols);
595  is_valid_ = false;
596 }
597 
598 #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:203
A point structure representing Euclidean xyz coordinates, and the RGB color.