Point Cloud Library (PCL)  1.14.1
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_
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
void setIndices(const IndicesPtr &indices) override
Provide a pointer to the vector of indices that represents the input data.
float getAngleStep() const
Returns the angle step.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:395
bool getAABB(PointT &min_point, PointT &max_point) const
This method gives access to the computed axis aligned bounding box.
float getPointMass() const
Returns the mass of point.
~MomentOfInertiaEstimation() override
Virtual destructor which frees the memory.
shared_ptr< Indices > IndicesPtr
Definition: pcl_base.h:58
bool getEigenVectors(Eigen::Vector3f &major, Eigen::Vector3f &middle, Eigen::Vector3f &minor) const
This method gives access to the computed eigen vectors.
bool getEccentricity(std::vector< float > &eccentricity) const
This method gives access to the computed ecentricities.
shared_ptr< const Indices > IndicesConstPtr
Definition: pcl_base.h:59
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
Implements the method for extracting features based on moment of inertia.
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
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
MomentOfInertiaEstimation()
Constructor that sets default values for member variables.
bool getMomentOfInertia(std::vector< float > &moment_of_inertia) const
This method gives access to the computed moments of inertia.
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Definition: distances.h:55
void setPointMass(const float point_mass)
This method allows to set point mass that will be used for moment of inertia calculation.
bool getEigenValues(float &major, float &middle, float &minor) const
This method gives access to the computed eigen values.
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:392
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
Definition: pcl_base.hpp:72
void setNormalizePointMassFlag(bool need_to_normalize)
This method allows to set the normalize_ flag.
Definition: norms.h:54
void compute()
This method launches the computation of all features.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:65
void setAngleStep(const float step)
This method allows to set the angle step.
A point structure representing Euclidean xyz coordinates, and the RGB color.
bool getMassCenter(Eigen::Vector3f &mass_center) const
This method gives access to the computed mass center.
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.
PointIndices::ConstPtr PointIndicesConstPtr
Definition: pcl_base.h:77
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: pcl_base.h:74
bool getNormalizePointMassFlag() const
Returns the normalize_ flag.