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>
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),
61 obb_position_ (0.0f, 0.0f, 0.0f)
66 template <
typename Po
intT>
69 moment_of_inertia_.clear ();
70 eccentricity_.clear ();
74 template <
typename Po
intT>
void
86 template <
typename Po
intT>
float
93 template <
typename Po
intT>
void
96 normalize_ = need_to_normalize;
102 template <
typename Po
intT>
bool
109 template <
typename Po
intT>
void
112 if (point_mass <= 0.0f)
115 point_mass_ = point_mass;
121 template <
typename Po
intT>
float
124 return (point_mass_);
128 template <
typename Po
intT>
void
131 moment_of_inertia_.clear ();
132 eccentricity_.clear ();
142 if (!indices_->empty ())
143 point_mass_ = 1.0f / static_cast <float> (indices_->size () * indices_->size ());
150 Eigen::Matrix <float, 3, 3> covariance_matrix;
151 covariance_matrix.setZero ();
154 computeEigenVectors (covariance_matrix, major_axis_, middle_axis_, minor_axis_, major_value_, middle_value_, minor_value_);
157 while (theta <= 90.0f)
160 Eigen::Vector3f rotated_vector;
161 rotateVector (major_axis_, middle_axis_, theta, rotated_vector);
162 while (phi <= 360.0f)
164 Eigen::Vector3f current_axis;
165 rotateVector (rotated_vector, minor_axis_, phi, current_axis);
166 current_axis.normalize ();
169 float current_moment_of_inertia = calculateMomentOfInertia (current_axis, mean_value_);
170 moment_of_inertia_.push_back (current_moment_of_inertia);
174 getProjectedCloud (current_axis, mean_value_, projected_cloud);
175 Eigen::Matrix <float, 3, 3> covariance_matrix;
176 covariance_matrix.setZero ();
178 projected_cloud.reset ();
179 float current_eccentricity = computeEccentricity (covariance_matrix, current_axis);
180 eccentricity_.push_back (current_eccentricity);
195 template <
typename Po
intT>
bool
198 min_point = aabb_min_point_;
199 max_point = aabb_max_point_;
205 template <
typename Po
intT>
bool
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_;
219 template <
typename Po
intT>
void
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 ();
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 ();
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++)
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);
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;
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;
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);
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);
261 obb_min_point_.x -= shift (0);
262 obb_min_point_.y -= shift (1);
263 obb_min_point_.z -= shift (2);
265 obb_max_point_.x -= shift (0);
266 obb_max_point_.y -= shift (1);
267 obb_max_point_.z -= shift (2);
269 obb_position_ = mean_value_ + obb_rotational_matrix_ * shift;
273 template <
typename Po
intT>
bool
276 major = major_value_;
277 middle = middle_value_;
278 minor = minor_value_;
284 template <
typename Po
intT>
bool
288 middle = middle_axis_;
295 template <
typename Po
intT>
bool
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 ());
305 template <
typename Po
intT>
bool
308 eccentricity.resize (eccentricity_.size (), 0.0f);
309 std::copy (eccentricity_.cbegin (), eccentricity_.cend (), eccentricity.begin ());
315 template <
typename Po
intT>
void
318 mean_value_ (0) = 0.0f;
319 mean_value_ (1) = 0.0f;
320 mean_value_ (2) = 0.0f;
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 ();
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 ();
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++)
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;
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;
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;
346 if (number_of_points == 0)
347 number_of_points = 1;
349 mean_value_ (0) /= number_of_points;
350 mean_value_ (1) /= number_of_points;
351 mean_value_ (2) /= number_of_points;
355 template <
typename Po
intT>
void
358 covariance_matrix.setZero ();
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++)
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);
369 covariance_matrix += current_point * current_point.transpose ();
372 covariance_matrix *= factor;
376 template <
typename Po
intT>
void
379 covariance_matrix.setZero ();
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++)
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);
390 covariance_matrix += current_point * current_point.transpose ();
393 covariance_matrix *= factor;
397 template <
typename Po
intT>
void
399 Eigen::Vector3f& major_axis, Eigen::Vector3f& middle_axis, Eigen::Vector3f& minor_axis,
float& major_value,
400 float& middle_value,
float& minor_value)
402 Eigen::EigenSolver <Eigen::Matrix <float, 3, 3> > eigen_solver;
403 eigen_solver.
compute (covariance_matrix);
405 Eigen::EigenSolver <Eigen::Matrix <float, 3, 3> >::EigenvectorsType eigen_vectors;
406 Eigen::EigenSolver <Eigen::Matrix <float, 3, 3> >::EigenvalueType eigen_values;
407 eigen_vectors = eigen_solver.eigenvectors ();
408 eigen_values = eigen_solver.eigenvalues ();
410 unsigned int temp = 0;
411 unsigned int major_index = 0;
412 unsigned int middle_index = 1;
413 unsigned int minor_index = 2;
415 if (eigen_values.real () (major_index) < eigen_values.real () (middle_index))
418 major_index = middle_index;
422 if (eigen_values.real () (major_index) < eigen_values.real () (minor_index))
425 major_index = minor_index;
429 if (eigen_values.real () (middle_index) < eigen_values.real () (minor_index))
432 minor_index = middle_index;
436 major_value = eigen_values.real () (major_index);
437 middle_value = eigen_values.real () (middle_index);
438 minor_value = eigen_values.real () (minor_index);
440 major_axis = eigen_vectors.col (major_index).real ();
441 middle_axis = eigen_vectors.col (middle_index).real ();
442 minor_axis = eigen_vectors.col (minor_index).real ();
444 major_axis.normalize ();
445 middle_axis.normalize ();
446 minor_axis.normalize ();
448 float det = major_axis.dot (middle_axis.cross (minor_axis));
451 major_axis (0) = -major_axis (0);
452 major_axis (1) = -major_axis (1);
453 major_axis (2) = -major_axis (2);
458 template <
typename Po
intT>
void
461 Eigen::Matrix <float, 3, 3> rotation_matrix;
462 const float x = axis (0);
463 const float y = axis (1);
464 const float z = axis (2);
465 const float rad = M_PI / 180.0f;
466 const float cosine = std::cos (angle * rad);
467 const float sine = std::sin (angle * rad);
468 rotation_matrix << cosine + (1 - cosine) * x * x, (1 - cosine) * x * y - sine * z, (1 - cosine) * x * z + sine * y,
469 (1 - cosine) * y * x + sine * z, cosine + (1 - cosine) * y * y, (1 - cosine) * y * z - sine * x,
470 (1 - cosine) * z * x - sine * y, (1 - cosine) * z * y + sine * x, cosine + (1 - cosine) * z * z;
472 rotated_vector = rotation_matrix * vector;
476 template <
typename Po
intT>
float
479 float moment_of_inertia = 0.0f;
480 auto number_of_points = static_cast <
unsigned int> (indices_->size ());
481 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
483 Eigen::Vector3f vector;
484 vector (0) = mean_value (0) - (*input_)[(*indices_)[i_point]].x;
485 vector (1) = mean_value (1) - (*input_)[(*indices_)[i_point]].y;
486 vector (2) = mean_value (2) - (*input_)[(*indices_)[i_point]].z;
488 Eigen::Vector3f product = vector.cross (current_axis);
490 float distance = product (0) * product (0) + product (1) * product (1) + product (2) * product (2);
492 moment_of_inertia += distance;
495 return (point_mass_ * moment_of_inertia);
499 template <
typename Po
intT>
void
502 const float D = - normal_vector.dot (point);
504 auto number_of_points = static_cast <
unsigned int> (indices_->size ());
505 projected_cloud->
points.resize (number_of_points,
PointT ());
507 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
509 const unsigned int index = (*indices_)[i_point];
510 float K = - (D + normal_vector (0) * (*input_)[index].x + normal_vector (1) * (*input_)[index].y + normal_vector (2) * (*input_)[index].z);
512 projected_point.x = (*input_)[index].x + K * normal_vector (0);
513 projected_point.y = (*input_)[index].y + K * normal_vector (1);
514 projected_point.z = (*input_)[index].z + K * normal_vector (2);
515 (*projected_cloud)[i_point] = projected_point;
517 projected_cloud->
width = number_of_points;
518 projected_cloud->
height = 1;
519 projected_cloud->
header = input_->header;
523 template <
typename Po
intT>
float
526 Eigen::Vector3f major_axis (0.0f, 0.0f, 0.0f);
527 Eigen::Vector3f middle_axis (0.0f, 0.0f, 0.0f);
528 Eigen::Vector3f minor_axis (0.0f, 0.0f, 0.0f);
529 float major_value = 0.0f;
530 float middle_value = 0.0f;
531 float minor_value = 0.0f;
532 computeEigenVectors (covariance_matrix, major_axis, middle_axis, minor_axis, major_value, middle_value, minor_value);
534 float major = std::abs (major_axis.dot (normal_vector));
535 float middle = std::abs (middle_axis.dot (normal_vector));
536 float minor = std::abs (minor_axis.dot (normal_vector));
538 float eccentricity = 0.0f;
540 if (major >= middle && major >= minor && middle_value != 0.0f)
541 eccentricity = std::pow (1.0f - (minor_value * minor_value) / (middle_value * middle_value), 0.5f);
543 if (middle >= major && middle >= minor && major_value != 0.0f)
544 eccentricity = std::pow (1.0f - (minor_value * minor_value) / (major_value * major_value), 0.5f);
546 if (minor >= major && minor >= middle && major_value != 0.0f)
547 eccentricity = std::pow (1.0f - (middle_value * middle_value) / (major_value * major_value), 0.5f);
549 return (eccentricity);
553 template <
typename Po
intT>
bool
556 mass_center = mean_value_;
562 template <
typename Po
intT>
void
570 template <
typename Po
intT>
void
578 template <
typename Po
intT>
void
586 template <
typename Po
intT>
void
594 template <
typename Po
intT>
void
601 #endif // PCL_MOMENT_OF_INERTIA_ESTIMATION_HPP_
shared_ptr< PointCloud< PointT > > Ptr
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.
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.
~MomentOfInertiaEstimation() override
Virtual destructor which frees the memory.
shared_ptr< Indices > IndicesPtr
bool getEigenVectors(Eigen::Vector3f &major, Eigen::Vector3f &middle, Eigen::Vector3f &minor) const
This method gives access to the computed eigen vectors.
bool getEccentricity(std::vector< float > &eccentricity) const
This method gives access to the computed ecentricities.
shared_ptr< const Indices > IndicesConstPtr
std::uint32_t width
The point cloud width (if organized as an image-structure).
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
Implements the method for extracting features based on moment of inertia.
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)
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.
std::uint32_t height
The point cloud height (if organized as an image-structure).
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.
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
void setNormalizePointMassFlag(bool need_to_normalize)
This method allows to set the normalize_ flag.
void compute()
This method launches the computation of all features.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
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.
PointIndices::ConstPtr PointIndicesConstPtr
typename PointCloud::ConstPtr PointCloudConstPtr
bool getNormalizePointMassFlag() const
Returns the normalize_ flag.