Point Cloud Library (PCL)  1.11.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>
50 pcl::MomentOfInertiaEstimation<PointT>::MomentOfInertiaEstimation () :
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>
72 pcl::MomentOfInertiaEstimation<PointT>::~MomentOfInertiaEstimation ()
73 {
74  moment_of_inertia_.clear ();
75  eccentricity_.clear ();
76 }
77 
78 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
79 template <typename PointT> void
80 pcl::MomentOfInertiaEstimation<PointT>::setAngleStep (const float step)
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
92 pcl::MomentOfInertiaEstimation<PointT>::getAngleStep () const
93 {
94  return (step_);
95 }
96 
97 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
98 template <typename PointT> void
99 pcl::MomentOfInertiaEstimation<PointT>::setNormalizePointMassFlag (bool need_to_normalize)
100 {
101  normalize_ = need_to_normalize;
102 
103  is_valid_ = false;
104 }
105 
106 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
107 template <typename PointT> bool
108 pcl::MomentOfInertiaEstimation<PointT>::getNormalizePointMassFlag () const
109 {
110  return (normalize_);
111 }
112 
113 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
114 template <typename PointT> void
115 pcl::MomentOfInertiaEstimation<PointT>::setPointMass (const float point_mass)
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
127 pcl::MomentOfInertiaEstimation<PointT>::getPointMass () const
128 {
129  return (point_mass_);
130 }
131 
132 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
133 template <typename PointT> void
134 pcl::MomentOfInertiaEstimation<PointT>::compute ()
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
201 pcl::MomentOfInertiaEstimation<PointT>::getAABB (PointT& min_point, PointT& max_point) const
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
225 pcl::MomentOfInertiaEstimation<PointT>::computeOBB ()
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_.begin (), moment_of_inertia_.end (), 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_.begin (), eccentricity_.end (), eccentricity.begin ());
315 
316  return (is_valid_);
317 }
318 
319 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
320 template <typename PointT> void
321 pcl::MomentOfInertiaEstimation<PointT>::computeMeanValue ()
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
568 pcl::MomentOfInertiaEstimation<PointT>::setInputCloud (const PointCloudConstPtr& cloud)
569 {
570  pcl::PCLBase<PointT>::setInputCloud (cloud);
571  is_valid_ = false;
572 }
573 
574 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
575 template <typename PointT> void
576 pcl::MomentOfInertiaEstimation<PointT>::setIndices (const IndicesPtr& indices)
577 {
578  pcl::PCLBase<PointT>::setIndices (indices);
579  is_valid_ = false;
580 }
581 
582 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
583 template <typename PointT> void
584 pcl::MomentOfInertiaEstimation<PointT>::setIndices (const IndicesConstPtr& indices)
585 {
586  pcl::PCLBase<PointT>::setIndices (indices);
587  is_valid_ = false;
588 }
589 
590 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
591 template <typename PointT> void
592 pcl::MomentOfInertiaEstimation<PointT>::setIndices (const PointIndicesConstPtr& indices)
593 {
594  pcl::PCLBase<PointT>::setIndices (indices);
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_
pcl::K
@ K
Definition: norms.h:54
pcl::IndicesPtr
shared_ptr< Indices > IndicesPtr
Definition: pcl_base.h:58
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:393
pcl::geometry::distance
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
pcl::PCLBase::PointCloudConstPtr
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: pcl_base.h:74
pcl::PointCloud::points
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:388
pcl::IndicesConstPtr
shared_ptr< const Indices > IndicesConstPtr
Definition: pcl_base.h:59
pcl::MomentOfInertiaEstimation::compute
void compute()
This method launches the computation of all features.
Definition: moment_of_inertia_estimation.hpp:134
pcl::PCLBase::PointIndicesConstPtr
PointIndices::ConstPtr PointIndicesConstPtr
Definition: pcl_base.h:77
pcl::MomentOfInertiaEstimation::getOBB
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.
Definition: moment_of_inertia_estimation.hpp:211
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: distances.h:55
pcl::PCLBase::setInputCloud
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:65
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:628
pcl::MomentOfInertiaEstimation::getAngleStep
float getAngleStep() const
Returns the angle step.
Definition: moment_of_inertia_estimation.hpp:92
pcl::MomentOfInertiaEstimation::getEigenValues
bool getEigenValues(float &major, float &middle, float &minor) const
This method gives access to the computed eigen values.
Definition: moment_of_inertia_estimation.hpp:279
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:391
pcl::MomentOfInertiaEstimation::setPointMass
void setPointMass(const float point_mass)
This method allows to set point mass that will be used for moment of inertia calculation.
Definition: moment_of_inertia_estimation.hpp:115
pcl::computeCovarianceMatrix
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:180
M_PI
#define M_PI
Definition: pcl_macros.h:201
pcl::MomentOfInertiaEstimation::getAABB
bool getAABB(PointT &min_point, PointT &max_point) const
This method gives access to the computed axis aligned bounding box.
Definition: moment_of_inertia_estimation.hpp:201
pcl::MomentOfInertiaEstimation::getMomentOfInertia
bool getMomentOfInertia(std::vector< float > &moment_of_inertia) const
This method gives access to the computed moments of inertia.
Definition: moment_of_inertia_estimation.hpp:301
pcl::MomentOfInertiaEstimation::setIndices
void setIndices(const IndicesPtr &indices) override
Provide a pointer to the vector of indices that represents the input data.
Definition: moment_of_inertia_estimation.hpp:576
pcl::PCLBase::setIndices
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
Definition: pcl_base.hpp:72
pcl::MomentOfInertiaEstimation::MomentOfInertiaEstimation
MomentOfInertiaEstimation()
Constructor that sets default values for member variables.
Definition: moment_of_inertia_estimation.hpp:50
pcl::MomentOfInertiaEstimation::~MomentOfInertiaEstimation
~MomentOfInertiaEstimation()
Virtual destructor which frees the memory.
Definition: moment_of_inertia_estimation.hpp:72
pcl::PointCloud::header
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:385
pcl::MomentOfInertiaEstimation::getMassCenter
bool getMassCenter(Eigen::Vector3f &mass_center) const
This method gives access to the computed mass center.
Definition: moment_of_inertia_estimation.hpp:559
pcl::MomentOfInertiaEstimation::getNormalizePointMassFlag
bool getNormalizePointMassFlag() const
Returns the normalize_ flag.
Definition: moment_of_inertia_estimation.hpp:108
pcl::MomentOfInertiaEstimation::getEigenVectors
bool getEigenVectors(Eigen::Vector3f &major, Eigen::Vector3f &middle, Eigen::Vector3f &minor) const
This method gives access to the computed eigen vectors.
Definition: moment_of_inertia_estimation.hpp:290
pcl::MomentOfInertiaEstimation::setInputCloud
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
Definition: moment_of_inertia_estimation.hpp:568
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:406
pcl::MomentOfInertiaEstimation::getPointMass
float getPointMass() const
Returns the mass of point.
Definition: moment_of_inertia_estimation.hpp:127
pcl::MomentOfInertiaEstimation::getEccentricity
bool getEccentricity(std::vector< float > &eccentricity) const
This method gives access to the computed ecentricities.
Definition: moment_of_inertia_estimation.hpp:311
pcl::MomentOfInertiaEstimation
Implements the method for extracting features based on moment of inertia.
Definition: moment_of_inertia_estimation.h:54
pcl::MomentOfInertiaEstimation::setNormalizePointMassFlag
void setNormalizePointMassFlag(bool need_to_normalize)
This method allows to set the normalize_ flag.
Definition: moment_of_inertia_estimation.hpp:99
pcl::MomentOfInertiaEstimation::setAngleStep
void setAngleStep(const float step)
This method allows to set the angle step.
Definition: moment_of_inertia_estimation.hpp:80