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_
Implements the method for extracting features based on moment of inertia.
float getAngleStep() const
Returns the angle step.
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.
~MomentOfInertiaEstimation()
Virtual destructor which frees the memory.
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.
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: pcl_base.h:77
PointIndices::ConstPtr PointIndicesConstPtr
Definition: pcl_base.h:80
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:181
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:414
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:408
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:416
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:429
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:411
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
@ 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:62
shared_ptr< Indices > IndicesPtr
Definition: pcl_base.h:61
#define M_PI
Definition: pcl_macros.h:192
A point structure representing Euclidean xyz coordinates, and the RGB color.