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 const Eigen::SelfAdjointEigenSolver<Eigen::Matrix<float, 3, 3>> eigen_solver(covariance_matrix);
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 ();
407 unsigned int temp = 0;
408 unsigned int major_index = 0;
409 unsigned int middle_index = 1;
410 unsigned int minor_index = 2;
412 if (eigen_values (major_index) < eigen_values (middle_index))
415 major_index = middle_index;
419 if (eigen_values (major_index) < eigen_values (minor_index))
422 major_index = minor_index;
426 if (eigen_values (middle_index) < eigen_values (minor_index))
429 minor_index = middle_index;
433 major_value = eigen_values (major_index);
434 middle_value = eigen_values (middle_index);
435 minor_value = eigen_values (minor_index);
437 major_axis = eigen_vectors.col (major_index);
438 middle_axis = eigen_vectors.col (middle_index);
439 minor_axis = eigen_vectors.col (minor_index);
441 major_axis.normalize ();
442 middle_axis.normalize ();
443 minor_axis.normalize ();
445 float det = major_axis.dot (middle_axis.cross (minor_axis));
448 major_axis (0) = -major_axis (0);
449 major_axis (1) = -major_axis (1);
450 major_axis (2) = -major_axis (2);
455 template <
typename Po
intT>
void
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;
469 rotated_vector = rotation_matrix * vector;
473 template <
typename Po
intT>
float
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++)
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;
485 Eigen::Vector3f product = vector.cross (current_axis);
487 float distance = product (0) * product (0) + product (1) * product (1) + product (2) * product (2);
492 return (point_mass_ * moment_of_inertia);
496 template <
typename Po
intT>
void
499 const float D = - normal_vector.dot (point);
501 auto number_of_points =
static_cast <unsigned int> (indices_->size ());
502 projected_cloud->
points.resize (number_of_points,
PointT ());
504 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
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);
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;
514 projected_cloud->
width = number_of_points;
515 projected_cloud->
height = 1;
516 projected_cloud->
header = input_->header;
520 template <
typename Po
intT>
float
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);
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));
535 float eccentricity = 0.0f;
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);
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);
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);
546 return (eccentricity);
550 template <
typename Po
intT>
bool
553 mass_center = mean_value_;
559 template <
typename Po
intT>
void
567 template <
typename Po
intT>
void
575 template <
typename Po
intT>
void
583 template <
typename Po
intT>
void
591 template <
typename Po
intT>
void
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.
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.