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