Point Cloud Library (PCL) 1.15.1-dev
Loading...
Searching...
No Matches
moment_of_inertia_estimation.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 *
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Willow Garage, Inc. nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *
35 * Author : Sergey Ushakov
36 * Email : sergey.s.ushakov@mail.ru
37 *
38 */
39
40#ifndef PCL_MOMENT_OF_INERTIA_ESTIMATION_HPP_
41#define PCL_MOMENT_OF_INERTIA_ESTIMATION_HPP_
42
43#include <Eigen/Eigenvalues> // for EigenSolver
44
45#include <pcl/features/moment_of_inertia_estimation.h>
46#include <pcl/features/feature.h>
47
48//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
49template <typename PointT>
51
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
57 aabb_min_point_ (),
58 aabb_max_point_ (),
59 obb_min_point_ (),
60 obb_max_point_ (),
61 obb_position_ (0.0f, 0.0f, 0.0f)
62{
63}
64
65//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
66template <typename PointT>
68{
69 moment_of_inertia_.clear ();
70 eccentricity_.clear ();
71}
72
73//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
74template <typename PointT> void
76{
77 if (step <= 0.0f)
78 return;
79
80 step_ = step;
81
82 is_valid_ = false;
83}
84
85//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
86template <typename PointT> float
88{
89 return (step_);
90}
91
92//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
93template <typename PointT> void
95{
96 normalize_ = need_to_normalize;
97
98 is_valid_ = false;
99}
100
101//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
102template <typename PointT> bool
107
108//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
109template <typename PointT> void
111{
112 if (point_mass <= 0.0f)
113 return;
114
115 point_mass_ = point_mass;
116
117 is_valid_ = false;
118}
119
120//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
121template <typename PointT> float
123{
124 return (point_mass_);
125}
126
127//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
128template <typename PointT> void
130{
131 moment_of_inertia_.clear ();
132 eccentricity_.clear ();
133
134 if (!initCompute ())
135 {
136 deinitCompute ();
137 return;
138 }
139
140 if (normalize_)
141 {
142 if (!indices_->empty ())
143 point_mass_ = 1.0f / static_cast <float> (indices_->size () * indices_->size ());
144 else
145 point_mass_ = 1.0f;
146 }
147
148 computeMeanValue ();
149
150 Eigen::Matrix <float, 3, 3> covariance_matrix;
151 covariance_matrix.setZero ();
152 computeCovarianceMatrix (covariance_matrix);
153
154 computeEigenVectors (covariance_matrix, major_axis_, middle_axis_, minor_axis_, major_value_, middle_value_, minor_value_);
155
156 float theta = 0.0f;
157 while (theta <= 90.0f)
158 {
159 float phi = 0.0f;
160 Eigen::Vector3f rotated_vector;
161 rotateVector (major_axis_, middle_axis_, theta, rotated_vector);
162 while (phi <= 360.0f)
163 {
164 Eigen::Vector3f current_axis;
165 rotateVector (rotated_vector, minor_axis_, phi, current_axis);
166 current_axis.normalize ();
167
168 //compute moment of inertia for the current axis
169 float current_moment_of_inertia = calculateMomentOfInertia (current_axis, mean_value_);
170 moment_of_inertia_.push_back (current_moment_of_inertia);
171
172 //compute eccentricity for the current plane
173 typename pcl::PointCloud<PointT>::Ptr projected_cloud (new pcl::PointCloud<PointT> ());
174 getProjectedCloud (current_axis, mean_value_, projected_cloud);
175 Eigen::Matrix <float, 3, 3> covariance_matrix;
176 covariance_matrix.setZero ();
177 computeCovarianceMatrix (projected_cloud, covariance_matrix);
178 projected_cloud.reset ();
179 float current_eccentricity = computeEccentricity (covariance_matrix, current_axis);
180 eccentricity_.push_back (current_eccentricity);
181
182 phi += step_;
183 }
184 theta += step_;
185 }
186
187 computeOBB ();
188
189 is_valid_ = true;
190
191 deinitCompute ();
192}
193
194//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
195template <typename PointT> bool
197{
198 min_point = aabb_min_point_;
199 max_point = aabb_max_point_;
200
201 return (is_valid_);
202}
203
204//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
205template <typename PointT> bool
206pcl::MomentOfInertiaEstimation<PointT>::getOBB (PointT& min_point, PointT& max_point, PointT& position, Eigen::Matrix3f& rotational_matrix) const
207{
208 min_point = obb_min_point_;
209 max_point = obb_max_point_;
210 position.x = obb_position_ (0);
211 position.y = obb_position_ (1);
212 position.z = obb_position_ (2);
213 rotational_matrix = obb_rotational_matrix_;
214
215 return (is_valid_);
216}
217
218//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
219template <typename PointT> void
221{
222 obb_min_point_.x = std::numeric_limits <float>::max ();
223 obb_min_point_.y = std::numeric_limits <float>::max ();
224 obb_min_point_.z = std::numeric_limits <float>::max ();
225
226 obb_max_point_.x = std::numeric_limits <float>::min ();
227 obb_max_point_.y = std::numeric_limits <float>::min ();
228 obb_max_point_.z = std::numeric_limits <float>::min ();
229
230 auto number_of_points = static_cast <unsigned int> (indices_->size ());
231 for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
232 {
233 float x = ((*input_)[(*indices_)[i_point]].x - mean_value_ (0)) * major_axis_ (0) +
234 ((*input_)[(*indices_)[i_point]].y - mean_value_ (1)) * major_axis_ (1) +
235 ((*input_)[(*indices_)[i_point]].z - mean_value_ (2)) * major_axis_ (2);
236 float y = ((*input_)[(*indices_)[i_point]].x - mean_value_ (0)) * middle_axis_ (0) +
237 ((*input_)[(*indices_)[i_point]].y - mean_value_ (1)) * middle_axis_ (1) +
238 ((*input_)[(*indices_)[i_point]].z - mean_value_ (2)) * middle_axis_ (2);
239 float z = ((*input_)[(*indices_)[i_point]].x - mean_value_ (0)) * minor_axis_ (0) +
240 ((*input_)[(*indices_)[i_point]].y - mean_value_ (1)) * minor_axis_ (1) +
241 ((*input_)[(*indices_)[i_point]].z - mean_value_ (2)) * minor_axis_ (2);
242
243 if (x <= obb_min_point_.x) obb_min_point_.x = x;
244 if (y <= obb_min_point_.y) obb_min_point_.y = y;
245 if (z <= obb_min_point_.z) obb_min_point_.z = z;
246
247 if (x >= obb_max_point_.x) obb_max_point_.x = x;
248 if (y >= obb_max_point_.y) obb_max_point_.y = y;
249 if (z >= obb_max_point_.z) obb_max_point_.z = z;
250 }
251
252 obb_rotational_matrix_ << major_axis_ (0), middle_axis_ (0), minor_axis_ (0),
253 major_axis_ (1), middle_axis_ (1), minor_axis_ (1),
254 major_axis_ (2), middle_axis_ (2), minor_axis_ (2);
255
256 Eigen::Vector3f shift (
257 (obb_max_point_.x + obb_min_point_.x) / 2.0f,
258 (obb_max_point_.y + obb_min_point_.y) / 2.0f,
259 (obb_max_point_.z + obb_min_point_.z) / 2.0f);
260
261 obb_min_point_.x -= shift (0);
262 obb_min_point_.y -= shift (1);
263 obb_min_point_.z -= shift (2);
264
265 obb_max_point_.x -= shift (0);
266 obb_max_point_.y -= shift (1);
267 obb_max_point_.z -= shift (2);
268
269 obb_position_.noalias() = mean_value_ + obb_rotational_matrix_ * shift;
270}
271
272//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
273template <typename PointT> bool
274pcl::MomentOfInertiaEstimation<PointT>::getEigenValues (float& major, float& middle, float& minor) const
275{
276 major = major_value_;
277 middle = middle_value_;
278 minor = minor_value_;
279
280 return (is_valid_);
281}
282
283//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
284template <typename PointT> bool
285pcl::MomentOfInertiaEstimation<PointT>::getEigenVectors (Eigen::Vector3f& major, Eigen::Vector3f& middle, Eigen::Vector3f& minor) const
286{
287 major = major_axis_;
288 middle = middle_axis_;
289 minor = minor_axis_;
290
291 return (is_valid_);
292}
293
294//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
295template <typename PointT> bool
296pcl::MomentOfInertiaEstimation<PointT>::getMomentOfInertia (std::vector <float>& moment_of_inertia) const
297{
298 moment_of_inertia.resize (moment_of_inertia_.size (), 0.0f);
299 std::copy (moment_of_inertia_.cbegin (), moment_of_inertia_.cend (), moment_of_inertia.begin ());
300
301 return (is_valid_);
302}
303
304//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
305template <typename PointT> bool
306pcl::MomentOfInertiaEstimation<PointT>::getEccentricity (std::vector <float>& eccentricity) const
307{
308 eccentricity.resize (eccentricity_.size (), 0.0f);
309 std::copy (eccentricity_.cbegin (), eccentricity_.cend (), eccentricity.begin ());
310
311 return (is_valid_);
312}
313
314//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
315template <typename PointT> void
317{
318 mean_value_ (0) = 0.0f;
319 mean_value_ (1) = 0.0f;
320 mean_value_ (2) = 0.0f;
321
322 aabb_min_point_.x = std::numeric_limits <float>::max ();
323 aabb_min_point_.y = std::numeric_limits <float>::max ();
324 aabb_min_point_.z = std::numeric_limits <float>::max ();
325
326 aabb_max_point_.x = -std::numeric_limits <float>::max ();
327 aabb_max_point_.y = -std::numeric_limits <float>::max ();
328 aabb_max_point_.z = -std::numeric_limits <float>::max ();
329
330 auto number_of_points = static_cast <unsigned int> (indices_->size ());
331 for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
332 {
333 mean_value_ (0) += (*input_)[(*indices_)[i_point]].x;
334 mean_value_ (1) += (*input_)[(*indices_)[i_point]].y;
335 mean_value_ (2) += (*input_)[(*indices_)[i_point]].z;
336
337 if ((*input_)[(*indices_)[i_point]].x <= aabb_min_point_.x) aabb_min_point_.x = (*input_)[(*indices_)[i_point]].x;
338 if ((*input_)[(*indices_)[i_point]].y <= aabb_min_point_.y) aabb_min_point_.y = (*input_)[(*indices_)[i_point]].y;
339 if ((*input_)[(*indices_)[i_point]].z <= aabb_min_point_.z) aabb_min_point_.z = (*input_)[(*indices_)[i_point]].z;
340
341 if ((*input_)[(*indices_)[i_point]].x >= aabb_max_point_.x) aabb_max_point_.x = (*input_)[(*indices_)[i_point]].x;
342 if ((*input_)[(*indices_)[i_point]].y >= aabb_max_point_.y) aabb_max_point_.y = (*input_)[(*indices_)[i_point]].y;
343 if ((*input_)[(*indices_)[i_point]].z >= aabb_max_point_.z) aabb_max_point_.z = (*input_)[(*indices_)[i_point]].z;
344 }
345
346 if (number_of_points == 0)
347 number_of_points = 1;
348
349 mean_value_ (0) /= number_of_points;
350 mean_value_ (1) /= number_of_points;
351 mean_value_ (2) /= number_of_points;
352}
353
354//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
355template <typename PointT> void
356pcl::MomentOfInertiaEstimation<PointT>::computeCovarianceMatrix (Eigen::Matrix <float, 3, 3>& covariance_matrix) const
357{
358 covariance_matrix.setZero ();
359
360 auto number_of_points = static_cast <unsigned int> (indices_->size ());
361 float factor = 1.0f / static_cast <float> ((number_of_points - 1 > 0)?(number_of_points - 1):1);
362 for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
363 {
364 Eigen::Vector3f current_point (0.0f, 0.0f, 0.0f);
365 current_point (0) = (*input_)[(*indices_)[i_point]].x - mean_value_ (0);
366 current_point (1) = (*input_)[(*indices_)[i_point]].y - mean_value_ (1);
367 current_point (2) = (*input_)[(*indices_)[i_point]].z - mean_value_ (2);
368
369 covariance_matrix.noalias() += current_point * current_point.transpose ();
370 }
371
372 covariance_matrix *= factor;
373}
374
375//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
376template <typename PointT> void
377pcl::MomentOfInertiaEstimation<PointT>::computeCovarianceMatrix (PointCloudConstPtr cloud, Eigen::Matrix <float, 3, 3>& covariance_matrix) const
378{
379 covariance_matrix.setZero ();
380
381 const auto number_of_points = cloud->size ();
382 float factor = 1.0f / static_cast <float> ((number_of_points - 1 > 0)?(number_of_points - 1):1);
383 Eigen::Vector3f current_point;
384 for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
385 {
386 current_point (0) = (*cloud)[i_point].x - mean_value_ (0);
387 current_point (1) = (*cloud)[i_point].y - mean_value_ (1);
388 current_point (2) = (*cloud)[i_point].z - mean_value_ (2);
389
390 covariance_matrix.noalias() += current_point * current_point.transpose ();
391 }
392
393 covariance_matrix *= factor;
394}
395
396//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
397template <typename PointT> void
398pcl::MomentOfInertiaEstimation<PointT>::computeEigenVectors (const Eigen::Matrix <float, 3, 3>& covariance_matrix,
399 Eigen::Vector3f& major_axis, Eigen::Vector3f& middle_axis, Eigen::Vector3f& minor_axis, float& major_value,
400 float& middle_value, float& minor_value)
401{
402 const Eigen::SelfAdjointEigenSolver<Eigen::Matrix<float, 3, 3>> eigen_solver(covariance_matrix);
403
404 const Eigen::SelfAdjointEigenSolver <Eigen::Matrix <float, 3, 3> >::EigenvectorsType& eigen_vectors = eigen_solver.eigenvectors ();
405 const Eigen::SelfAdjointEigenSolver <Eigen::Matrix <float, 3, 3> >::RealVectorType& eigen_values = eigen_solver.eigenvalues ();
406
407 unsigned int temp = 0;
408 unsigned int major_index = 0;
409 unsigned int middle_index = 1;
410 unsigned int minor_index = 2;
411
412 if (eigen_values (major_index) < eigen_values (middle_index))
413 {
414 temp = major_index;
415 major_index = middle_index;
416 middle_index = temp;
417 }
418
419 if (eigen_values (major_index) < eigen_values (minor_index))
420 {
421 temp = major_index;
422 major_index = minor_index;
423 minor_index = temp;
424 }
425
426 if (eigen_values (middle_index) < eigen_values (minor_index))
427 {
428 temp = minor_index;
429 minor_index = middle_index;
430 middle_index = temp;
431 }
432
433 major_value = eigen_values (major_index);
434 middle_value = eigen_values (middle_index);
435 minor_value = eigen_values (minor_index);
436
437 major_axis = eigen_vectors.col (major_index);
438 middle_axis = eigen_vectors.col (middle_index);
439 minor_axis = eigen_vectors.col (minor_index);
440
441 major_axis.normalize ();
442 middle_axis.normalize ();
443 minor_axis.normalize ();
444
445 float det = major_axis.dot (middle_axis.cross (minor_axis));
446 if (det <= 0.0f)
447 {
448 major_axis (0) = -major_axis (0);
449 major_axis (1) = -major_axis (1);
450 major_axis (2) = -major_axis (2);
451 }
452}
453
454//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
455template <typename PointT> void
456pcl::MomentOfInertiaEstimation<PointT>::rotateVector (const Eigen::Vector3f& vector, const Eigen::Vector3f& axis, const float angle, Eigen::Vector3f& rotated_vector) const
457{
458 Eigen::Matrix <float, 3, 3> rotation_matrix;
459 const float x = axis (0);
460 const float y = axis (1);
461 const float z = axis (2);
462 const float rad = M_PI / 180.0f;
463 const float cosine = std::cos (angle * rad);
464 const float sine = std::sin (angle * rad);
465 rotation_matrix << cosine + (1 - cosine) * x * x, (1 - cosine) * x * y - sine * z, (1 - cosine) * x * z + sine * y,
466 (1 - cosine) * y * x + sine * z, cosine + (1 - cosine) * y * y, (1 - cosine) * y * z - sine * x,
467 (1 - cosine) * z * x - sine * y, (1 - cosine) * z * y + sine * x, cosine + (1 - cosine) * z * z;
468
469 rotated_vector = rotation_matrix * vector;
470}
471
472//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
473template <typename PointT> float
474pcl::MomentOfInertiaEstimation<PointT>::calculateMomentOfInertia (const Eigen::Vector3f& current_axis, const Eigen::Vector3f& mean_value) const
475{
476 float moment_of_inertia = 0.0f;
477 auto number_of_points = static_cast <unsigned int> (indices_->size ());
478 for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
479 {
480 Eigen::Vector3f vector;
481 vector (0) = mean_value (0) - (*input_)[(*indices_)[i_point]].x;
482 vector (1) = mean_value (1) - (*input_)[(*indices_)[i_point]].y;
483 vector (2) = mean_value (2) - (*input_)[(*indices_)[i_point]].z;
484
485 Eigen::Vector3f product = vector.cross (current_axis);
486
487 float distance = product (0) * product (0) + product (1) * product (1) + product (2) * product (2);
488
489 moment_of_inertia += distance;
490 }
491
492 return (point_mass_ * moment_of_inertia);
493}
494
495//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
496template <typename PointT> void
497pcl::MomentOfInertiaEstimation<PointT>::getProjectedCloud (const Eigen::Vector3f& normal_vector, const Eigen::Vector3f& point, typename pcl::PointCloud <PointT>::Ptr projected_cloud) const
498{
499 const float D = - normal_vector.dot (point);
500
501 auto number_of_points = static_cast <unsigned int> (indices_->size ());
502 projected_cloud->points.resize (number_of_points, PointT ());
503
504 for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
505 {
506 const unsigned int index = (*indices_)[i_point];
507 float K = - (D + normal_vector (0) * (*input_)[index].x + normal_vector (1) * (*input_)[index].y + normal_vector (2) * (*input_)[index].z);
508 PointT projected_point;
509 projected_point.x = (*input_)[index].x + K * normal_vector (0);
510 projected_point.y = (*input_)[index].y + K * normal_vector (1);
511 projected_point.z = (*input_)[index].z + K * normal_vector (2);
512 (*projected_cloud)[i_point] = projected_point;
513 }
514 projected_cloud->width = number_of_points;
515 projected_cloud->height = 1;
516 projected_cloud->header = input_->header;
517}
518
519//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
520template <typename PointT> float
521pcl::MomentOfInertiaEstimation<PointT>::computeEccentricity (const Eigen::Matrix <float, 3, 3>& covariance_matrix, const Eigen::Vector3f& normal_vector)
522{
523 Eigen::Vector3f major_axis (0.0f, 0.0f, 0.0f);
524 Eigen::Vector3f middle_axis (0.0f, 0.0f, 0.0f);
525 Eigen::Vector3f minor_axis (0.0f, 0.0f, 0.0f);
526 float major_value = 0.0f;
527 float middle_value = 0.0f;
528 float minor_value = 0.0f;
529 computeEigenVectors (covariance_matrix, major_axis, middle_axis, minor_axis, major_value, middle_value, minor_value);
530
531 float major = std::abs (major_axis.dot (normal_vector));
532 float middle = std::abs (middle_axis.dot (normal_vector));
533 float minor = std::abs (minor_axis.dot (normal_vector));
534
535 float eccentricity = 0.0f;
536
537 if (major >= middle && major >= minor && middle_value != 0.0f)
538 eccentricity = std::pow (1.0f - (minor_value * minor_value) / (middle_value * middle_value), 0.5f);
539
540 if (middle >= major && middle >= minor && major_value != 0.0f)
541 eccentricity = std::pow (1.0f - (minor_value * minor_value) / (major_value * major_value), 0.5f);
542
543 if (minor >= major && minor >= middle && major_value != 0.0f)
544 eccentricity = std::pow (1.0f - (middle_value * middle_value) / (major_value * major_value), 0.5f);
545
546 return (eccentricity);
547}
548
549//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
550template <typename PointT> bool
552{
553 mass_center = mean_value_;
554
555 return (is_valid_);
556}
557
558//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
559template <typename PointT> void
565
566//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
567template <typename PointT> void
569{
571 is_valid_ = false;
572}
573
574//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
575template <typename PointT> void
577{
579 is_valid_ = false;
580}
581
582//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
583template <typename PointT> void
589
590//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
591template <typename PointT> void
592pcl::MomentOfInertiaEstimation<PointT>::setIndices (std::size_t row_start, std::size_t col_start, std::size_t nb_rows, std::size_t nb_cols)
593{
594 pcl::PCLBase<PointT>::setIndices (row_start, col_start, nb_rows, nb_cols);
595 is_valid_ = false;
596}
597
598#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.
~MomentOfInertiaEstimation() override
Virtual destructor which frees the memory.
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.
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.
typename pcl::PCLBase< PointT >::PointCloudConstPtr PointCloudConstPtr
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.
typename pcl::PCLBase< PointT >::PointIndicesConstPtr PointIndicesConstPtr
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.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition pcl_base.hpp:65
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
Definition pcl_base.hpp:72
PointCloud represents the base class in PCL for storing collections of 3D points.
std::uint32_t width
The point cloud width (if organized as an image-structure).
pcl::PCLHeader header
The point cloud header.
std::uint32_t height
The point cloud height (if organized as an image-structure).
shared_ptr< PointCloud< PointT > > Ptr
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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:192
@ 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:59
shared_ptr< Indices > IndicesPtr
Definition pcl_base.h:58
#define M_PI
Definition pcl_macros.h:201
A point structure representing Euclidean xyz coordinates, and the RGB color.