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