40 #ifndef PCL_MOMENT_OF_INERTIA_ESTIMATION_HPP_
41 #define PCL_MOMENT_OF_INERTIA_ESTIMATION_HPP_
43 #include <Eigen/Eigenvalues>
45 #include <pcl/features/moment_of_inertia_estimation.h>
46 #include <pcl/features/feature.h>
49 template <
typename Po
intT>
53 point_mass_ (0.0001f),
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),
66 obb_position_ (0.0f, 0.0f, 0.0f)
71 template <
typename Po
intT>
74 moment_of_inertia_.clear ();
75 eccentricity_.clear ();
79 template <
typename Po
intT>
void
91 template <
typename Po
intT>
float
98 template <
typename Po
intT>
void
101 normalize_ = need_to_normalize;
107 template <
typename Po
intT>
bool
114 template <
typename Po
intT>
void
117 if (point_mass <= 0.0f)
120 point_mass_ = point_mass;
126 template <
typename Po
intT>
float
129 return (point_mass_);
133 template <
typename Po
intT>
void
136 moment_of_inertia_.clear ();
137 eccentricity_.clear ();
147 if (!indices_->empty ())
148 point_mass_ = 1.0f /
static_cast <float> (indices_->size () * indices_->size ());
155 Eigen::Matrix <float, 3, 3> covariance_matrix;
156 covariance_matrix.setZero ();
159 computeEigenVectors (covariance_matrix, major_axis_, middle_axis_, minor_axis_, major_value_, middle_value_, minor_value_);
162 while (theta <= 90.0f)
165 Eigen::Vector3f rotated_vector;
166 rotateVector (major_axis_, middle_axis_, theta, rotated_vector);
167 while (phi <= 360.0f)
169 Eigen::Vector3f current_axis;
170 rotateVector (rotated_vector, minor_axis_, phi, current_axis);
171 current_axis.normalize ();
174 float current_moment_of_inertia = calculateMomentOfInertia (current_axis, mean_value_);
175 moment_of_inertia_.push_back (current_moment_of_inertia);
179 getProjectedCloud (current_axis, mean_value_, projected_cloud);
180 Eigen::Matrix <float, 3, 3> covariance_matrix;
181 covariance_matrix.setZero ();
183 projected_cloud.reset ();
184 float current_eccentricity = computeEccentricity (covariance_matrix, current_axis);
185 eccentricity_.push_back (current_eccentricity);
200 template <
typename Po
intT>
bool
203 min_point = aabb_min_point_;
204 max_point = aabb_max_point_;
210 template <
typename Po
intT>
bool
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_;
224 template <
typename Po
intT>
void
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 ();
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 ();
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++)
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);
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;
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;
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);
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);
266 obb_min_point_.x -= shift (0);
267 obb_min_point_.y -= shift (1);
268 obb_min_point_.z -= shift (2);
270 obb_max_point_.x -= shift (0);
271 obb_max_point_.y -= shift (1);
272 obb_max_point_.z -= shift (2);
274 obb_position_ = mean_value_ + obb_rotational_matrix_ * shift;
278 template <
typename Po
intT>
bool
281 major = major_value_;
282 middle = middle_value_;
283 minor = minor_value_;
289 template <
typename Po
intT>
bool
293 middle = middle_axis_;
300 template <
typename Po
intT>
bool
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 ());
310 template <
typename Po
intT>
bool
313 eccentricity.resize (eccentricity_.size (), 0.0f);
314 std::copy (eccentricity_.begin (), eccentricity_.end (), eccentricity.begin ());
320 template <
typename Po
intT>
void
323 mean_value_ (0) = 0.0f;
324 mean_value_ (1) = 0.0f;
325 mean_value_ (2) = 0.0f;
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 ();
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 ();
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++)
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;
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;
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;
351 if (number_of_points == 0)
352 number_of_points = 1;
354 mean_value_ (0) /= number_of_points;
355 mean_value_ (1) /= number_of_points;
356 mean_value_ (2) /= number_of_points;
360 template <
typename Po
intT>
void
363 covariance_matrix.setZero ();
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++)
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);
374 covariance_matrix += current_point * current_point.transpose ();
377 covariance_matrix *= factor;
381 template <
typename Po
intT>
void
384 covariance_matrix.setZero ();
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++)
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);
395 covariance_matrix += current_point * current_point.transpose ();
398 covariance_matrix *= factor;
402 template <
typename Po
intT>
void
404 Eigen::Vector3f& major_axis, Eigen::Vector3f& middle_axis, Eigen::Vector3f& minor_axis,
float& major_value,
405 float& middle_value,
float& minor_value)
407 Eigen::EigenSolver <Eigen::Matrix <float, 3, 3> > eigen_solver;
408 eigen_solver.
compute (covariance_matrix);
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 ();
415 unsigned int temp = 0;
416 unsigned int major_index = 0;
417 unsigned int middle_index = 1;
418 unsigned int minor_index = 2;
420 if (eigen_values.real () (major_index) < eigen_values.real () (middle_index))
423 major_index = middle_index;
427 if (eigen_values.real () (major_index) < eigen_values.real () (minor_index))
430 major_index = minor_index;
434 if (eigen_values.real () (middle_index) < eigen_values.real () (minor_index))
437 minor_index = middle_index;
441 major_value = eigen_values.real () (major_index);
442 middle_value = eigen_values.real () (middle_index);
443 minor_value = eigen_values.real () (minor_index);
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 ();
449 major_axis.normalize ();
450 middle_axis.normalize ();
451 minor_axis.normalize ();
453 float det = major_axis.dot (middle_axis.cross (minor_axis));
456 major_axis (0) = -major_axis (0);
457 major_axis (1) = -major_axis (1);
458 major_axis (2) = -major_axis (2);
463 template <
typename Po
intT>
void
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;
477 rotated_vector = rotation_matrix * vector;
481 template <
typename Po
intT>
float
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++)
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;
493 Eigen::Vector3f product = vector.cross (current_axis);
495 float distance = product (0) * product (0) + product (1) * product (1) + product (2) * product (2);
500 return (point_mass_ * moment_of_inertia);
504 template <
typename Po
intT>
void
507 const float D = - normal_vector.dot (point);
509 unsigned int number_of_points =
static_cast <unsigned int> (indices_->size ());
510 projected_cloud->
points.resize (number_of_points,
PointT ());
512 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
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);
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;
522 projected_cloud->
width = number_of_points;
523 projected_cloud->
height = 1;
524 projected_cloud->
header = input_->header;
528 template <
typename Po
intT>
float
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);
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));
543 float eccentricity = 0.0f;
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);
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);
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);
554 return (eccentricity);
558 template <
typename Po
intT>
bool
561 mass_center = mean_value_;
567 template <
typename Po
intT>
void
575 template <
typename Po
intT>
void
583 template <
typename Po
intT>
void
591 template <
typename Po
intT>
void
599 template <
typename Po
intT>
void
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.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
typename PointCloud::ConstPtr PointCloudConstPtr
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
PointIndices::ConstPtr PointIndicesConstPtr
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 > ¢roid, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix)
Compute the 3x3 covariance matrix of a given set of points.
float distance(const PointT &p1, const PointT &p2)
shared_ptr< const Indices > IndicesConstPtr
shared_ptr< Indices > IndicesPtr
A point structure representing Euclidean xyz coordinates, and the RGB color.