Point Cloud Library (PCL)  1.9.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 <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 (size_t row_start, size_t col_start, size_t nb_rows, 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  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  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(size_t i = row_start; i < row_end; i++)
638  for(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_
void setIndices(const IndicesPtr &indices) override
Provide a pointer to the vector of indices that represents the input data.
float getAngleStep() const
Returns the angle step.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:409
bool getAABB(PointT &min_point, PointT &max_point) const
This method gives access to the computed axis aligned bounding box.
float getPointMass() const
Returns the mass of point.
bool getEigenVectors(Eigen::Vector3f &major, Eigen::Vector3f &middle, Eigen::Vector3f &minor) const
This method gives access to the computed eigen vectors.
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition: pcl_base.h:154
bool getEccentricity(std::vector< float > &eccentricity) const
This method gives access to the computed ecentricities.
bool use_indices_
Set to true if point indices are used.
Definition: pcl_base.h:157
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:414
bool initCompute()
This method should get called before starting the actual computation.
Definition: pcl_base.hpp:138
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:412
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
boost::shared_ptr< Indices > IndicesPtr
Definition: pcl_base.h:61
Implements the method for extracting features based on moment of inertia.
MomentOfInertiaEstimation()
Constructor that sets default values for member variables.
bool getMomentOfInertia(std::vector< float > &moment_of_inertia) const
This method gives access to the computed moments of inertia.
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:427
boost::shared_ptr< PointIndices const > PointIndicesConstPtr
Definition: pcl_base.h:77
boost::shared_ptr< const Indices > IndicesConstPtr
Definition: pcl_base.h:62
bool deinitCompute()
This method should get called after finishing the actual computation.
Definition: pcl_base.hpp:173
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void setPointMass(const float point_mass)
This method allows to set point mass that will be used for moment of inertia calculation.
bool getEigenValues(float &major, float &middle, float &minor) const
This method gives access to the computed eigen values.
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:406
~MomentOfInertiaEstimation()
Virtual destructor which frees the memory.
void setNormalizePointMassFlag(bool need_to_normalize)
This method allows to set the normalize_ flag.
Definition: norms.h:54
void compute()
This method launches the computation of all features.
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:151
void setAngleStep(const float step)
This method allows to set the angle step.
A point structure representing Euclidean xyz coordinates, and the RGB color.
bool getMassCenter(Eigen::Vector3f &mass_center) const
This method gives access to the computed mass center.
bool getOBB(PointT &min_point, PointT &max_point, PointT &position, Eigen::Matrix3f &rotational_matrix) const
This method gives access to the computed oriented bounding box.
bool fake_indices_
If no set of indices are given, we construct a set of fake indices that mimic the input PointCloud...
Definition: pcl_base.h:160
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: pcl_base.h:74
bool getNormalizePointMassFlag() const
Returns the normalize_ flag.