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