40 #ifndef PCL_MOMENT_OF_INERTIA_ESTIMATION_HPP_
41 #define PCL_MOMENT_OF_INERTIA_ESTIMATION_HPP_
43 #include <pcl/features/moment_of_inertia_estimation.h>
46 template <
typename Po
intT>
50 point_mass_ (0.0001f),
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),
63 obb_position_ (0.0f, 0.0f, 0.0f)
68 template <
typename Po
intT>
71 moment_of_inertia_.clear ();
72 eccentricity_.clear ();
76 template <
typename Po
intT>
void
88 template <
typename Po
intT>
float
95 template <
typename Po
intT>
void
98 normalize_ = need_to_normalize;
104 template <
typename Po
intT>
bool
111 template <
typename Po
intT>
void
114 if (point_mass <= 0.0f)
117 point_mass_ = point_mass;
123 template <
typename Po
intT>
float
126 return (point_mass_);
130 template <
typename Po
intT>
void
133 moment_of_inertia_.clear ();
134 eccentricity_.clear ();
144 if (!indices_->empty ())
145 point_mass_ = 1.0f /
static_cast <float> (indices_->size () * indices_->size ());
152 Eigen::Matrix <float, 3, 3> covariance_matrix;
153 covariance_matrix.setZero ();
156 computeEigenVectors (covariance_matrix, major_axis_, middle_axis_, minor_axis_, major_value_, middle_value_, minor_value_);
159 while (theta <= 90.0f)
162 Eigen::Vector3f rotated_vector;
163 rotateVector (major_axis_, middle_axis_, theta, rotated_vector);
164 while (phi <= 360.0f)
166 Eigen::Vector3f current_axis;
167 rotateVector (rotated_vector, minor_axis_, phi, current_axis);
168 current_axis.normalize ();
171 float current_moment_of_inertia = calculateMomentOfInertia (current_axis, mean_value_);
172 moment_of_inertia_.push_back (current_moment_of_inertia);
176 getProjectedCloud (current_axis, mean_value_, projected_cloud);
177 Eigen::Matrix <float, 3, 3> covariance_matrix;
178 covariance_matrix.setZero ();
180 projected_cloud.reset ();
181 float current_eccentricity = computeEccentricity (covariance_matrix, current_axis);
182 eccentricity_.push_back (current_eccentricity);
197 template <
typename Po
intT>
bool
200 min_point = aabb_min_point_;
201 max_point = aabb_max_point_;
207 template <
typename Po
intT>
bool
210 min_point = obb_min_point_;
211 max_point = obb_max_point_;
212 position.x = obb_position_ (0);
213 position.y = obb_position_ (1);
214 position.z = obb_position_ (2);
215 rotational_matrix = obb_rotational_matrix_;
221 template <
typename Po
intT>
void
224 obb_min_point_.x = std::numeric_limits <float>::max ();
225 obb_min_point_.y = std::numeric_limits <float>::max ();
226 obb_min_point_.z = std::numeric_limits <float>::max ();
228 obb_max_point_.x = std::numeric_limits <float>::min ();
229 obb_max_point_.y = std::numeric_limits <float>::min ();
230 obb_max_point_.z = std::numeric_limits <float>::min ();
232 unsigned int number_of_points =
static_cast <unsigned int> (indices_->size ());
233 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
235 float x = (input_->points[(*indices_)[i_point]].x - mean_value_ (0)) * major_axis_ (0) +
236 (input_->points[(*indices_)[i_point]].y - mean_value_ (1)) * major_axis_ (1) +
237 (input_->points[(*indices_)[i_point]].z - mean_value_ (2)) * major_axis_ (2);
238 float y = (input_->points[(*indices_)[i_point]].x - mean_value_ (0)) * middle_axis_ (0) +
239 (input_->points[(*indices_)[i_point]].y - mean_value_ (1)) * middle_axis_ (1) +
240 (input_->points[(*indices_)[i_point]].z - mean_value_ (2)) * middle_axis_ (2);
241 float z = (input_->points[(*indices_)[i_point]].x - mean_value_ (0)) * minor_axis_ (0) +
242 (input_->points[(*indices_)[i_point]].y - mean_value_ (1)) * minor_axis_ (1) +
243 (input_->points[(*indices_)[i_point]].z - mean_value_ (2)) * minor_axis_ (2);
245 if (x <= obb_min_point_.x) obb_min_point_.x = x;
246 if (y <= obb_min_point_.y) obb_min_point_.y = y;
247 if (z <= obb_min_point_.z) obb_min_point_.z = z;
249 if (x >= obb_max_point_.x) obb_max_point_.x = x;
250 if (y >= obb_max_point_.y) obb_max_point_.y = y;
251 if (z >= obb_max_point_.z) obb_max_point_.z = z;
254 obb_rotational_matrix_ << major_axis_ (0), middle_axis_ (0), minor_axis_ (0),
255 major_axis_ (1), middle_axis_ (1), minor_axis_ (1),
256 major_axis_ (2), middle_axis_ (2), minor_axis_ (2);
258 Eigen::Vector3f shift (
259 (obb_max_point_.x + obb_min_point_.x) / 2.0f,
260 (obb_max_point_.y + obb_min_point_.y) / 2.0f,
261 (obb_max_point_.z + obb_min_point_.z) / 2.0f);
263 obb_min_point_.x -= shift (0);
264 obb_min_point_.y -= shift (1);
265 obb_min_point_.z -= shift (2);
267 obb_max_point_.x -= shift (0);
268 obb_max_point_.y -= shift (1);
269 obb_max_point_.z -= shift (2);
271 obb_position_ = mean_value_ + obb_rotational_matrix_ * shift;
275 template <
typename Po
intT>
bool
278 major = major_value_;
279 middle = middle_value_;
280 minor = minor_value_;
286 template <
typename Po
intT>
bool
290 middle = middle_axis_;
297 template <
typename Po
intT>
bool
300 moment_of_inertia.resize (moment_of_inertia_.size (), 0.0f);
301 std::copy (moment_of_inertia_.begin (), moment_of_inertia_.end (), moment_of_inertia.begin ());
307 template <
typename Po
intT>
bool
310 eccentricity.resize (eccentricity_.size (), 0.0f);
311 std::copy (eccentricity_.begin (), eccentricity_.end (), eccentricity.begin ());
317 template <
typename Po
intT>
void
320 mean_value_ (0) = 0.0f;
321 mean_value_ (1) = 0.0f;
322 mean_value_ (2) = 0.0f;
324 aabb_min_point_.x = std::numeric_limits <float>::max ();
325 aabb_min_point_.y = std::numeric_limits <float>::max ();
326 aabb_min_point_.z = std::numeric_limits <float>::max ();
328 aabb_max_point_.x = -std::numeric_limits <float>::max ();
329 aabb_max_point_.y = -std::numeric_limits <float>::max ();
330 aabb_max_point_.z = -std::numeric_limits <float>::max ();
332 unsigned int number_of_points =
static_cast <unsigned int> (indices_->size ());
333 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
335 mean_value_ (0) += input_->points[(*indices_)[i_point]].x;
336 mean_value_ (1) += input_->points[(*indices_)[i_point]].y;
337 mean_value_ (2) += input_->points[(*indices_)[i_point]].z;
339 if (input_->points[(*indices_)[i_point]].x <= aabb_min_point_.x) aabb_min_point_.x = input_->points[(*indices_)[i_point]].x;
340 if (input_->points[(*indices_)[i_point]].y <= aabb_min_point_.y) aabb_min_point_.y = input_->points[(*indices_)[i_point]].y;
341 if (input_->points[(*indices_)[i_point]].z <= aabb_min_point_.z) aabb_min_point_.z = input_->points[(*indices_)[i_point]].z;
343 if (input_->points[(*indices_)[i_point]].x >= aabb_max_point_.x) aabb_max_point_.x = input_->points[(*indices_)[i_point]].x;
344 if (input_->points[(*indices_)[i_point]].y >= aabb_max_point_.y) aabb_max_point_.y = input_->points[(*indices_)[i_point]].y;
345 if (input_->points[(*indices_)[i_point]].z >= aabb_max_point_.z) aabb_max_point_.z = input_->points[(*indices_)[i_point]].z;
348 if (number_of_points == 0)
349 number_of_points = 1;
351 mean_value_ (0) /= number_of_points;
352 mean_value_ (1) /= number_of_points;
353 mean_value_ (2) /= number_of_points;
357 template <
typename Po
intT>
void
360 covariance_matrix.setZero ();
362 unsigned int number_of_points =
static_cast <unsigned int> (indices_->size ());
363 float factor = 1.0f /
static_cast <float> ((number_of_points - 1 > 0)?(number_of_points - 1):1);
364 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
366 Eigen::Vector3f current_point (0.0f, 0.0f, 0.0f);
367 current_point (0) = input_->points[(*indices_)[i_point]].x - mean_value_ (0);
368 current_point (1) = input_->points[(*indices_)[i_point]].y - mean_value_ (1);
369 current_point (2) = input_->points[(*indices_)[i_point]].z - mean_value_ (2);
371 covariance_matrix += current_point * current_point.transpose ();
374 covariance_matrix *= factor;
378 template <
typename Po
intT>
void
381 covariance_matrix.setZero ();
383 unsigned int number_of_points =
static_cast <unsigned int> (cloud->points.size ());
384 float factor = 1.0f /
static_cast <float> ((number_of_points - 1 > 0)?(number_of_points - 1):1);
385 Eigen::Vector3f current_point;
386 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
388 current_point (0) = cloud->points[i_point].x - mean_value_ (0);
389 current_point (1) = cloud->points[i_point].y - mean_value_ (1);
390 current_point (2) = cloud->points[i_point].z - mean_value_ (2);
392 covariance_matrix += current_point * current_point.transpose ();
395 covariance_matrix *= factor;
399 template <
typename Po
intT>
void
401 Eigen::Vector3f& major_axis, Eigen::Vector3f& middle_axis, Eigen::Vector3f& minor_axis,
float& major_value,
402 float& middle_value,
float& minor_value)
404 Eigen::EigenSolver <Eigen::Matrix <float, 3, 3> > eigen_solver;
405 eigen_solver.
compute (covariance_matrix);
407 Eigen::EigenSolver <Eigen::Matrix <float, 3, 3> >::EigenvectorsType eigen_vectors;
408 Eigen::EigenSolver <Eigen::Matrix <float, 3, 3> >::EigenvalueType eigen_values;
409 eigen_vectors = eigen_solver.eigenvectors ();
410 eigen_values = eigen_solver.eigenvalues ();
412 unsigned int temp = 0;
413 unsigned int major_index = 0;
414 unsigned int middle_index = 1;
415 unsigned int minor_index = 2;
417 if (eigen_values.real () (major_index) < eigen_values.real () (middle_index))
420 major_index = middle_index;
424 if (eigen_values.real () (major_index) < eigen_values.real () (minor_index))
427 major_index = minor_index;
431 if (eigen_values.real () (middle_index) < eigen_values.real () (minor_index))
434 minor_index = middle_index;
438 major_value = eigen_values.real () (major_index);
439 middle_value = eigen_values.real () (middle_index);
440 minor_value = eigen_values.real () (minor_index);
442 major_axis = eigen_vectors.col (major_index).real ();
443 middle_axis = eigen_vectors.col (middle_index).real ();
444 minor_axis = eigen_vectors.col (minor_index).real ();
446 major_axis.normalize ();
447 middle_axis.normalize ();
448 minor_axis.normalize ();
450 float det = major_axis.dot (middle_axis.cross (minor_axis));
453 major_axis (0) = -major_axis (0);
454 major_axis (1) = -major_axis (1);
455 major_axis (2) = -major_axis (2);
460 template <
typename Po
intT>
void
463 Eigen::Matrix <float, 3, 3> rotation_matrix;
464 const float x = axis (0);
465 const float y = axis (1);
466 const float z = axis (2);
467 const float rad =
M_PI / 180.0f;
468 const float cosine = std::cos (angle * rad);
469 const float sine = std::sin (angle * rad);
470 rotation_matrix << cosine + (1 - cosine) * x * x, (1 - cosine) * x * y - sine * z, (1 - cosine) * x * z + sine * y,
471 (1 - cosine) * y * x + sine * z, cosine + (1 - cosine) * y * y, (1 - cosine) * y * z - sine * x,
472 (1 - cosine) * z * x - sine * y, (1 - cosine) * z * y + sine * x, cosine + (1 - cosine) * z * z;
474 rotated_vector = rotation_matrix * vector;
478 template <
typename Po
intT>
float
481 float moment_of_inertia = 0.0f;
482 unsigned int number_of_points =
static_cast <unsigned int> (indices_->size ());
483 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
485 Eigen::Vector3f vector;
486 vector (0) = mean_value (0) - input_->points[(*indices_)[i_point]].x;
487 vector (1) = mean_value (1) - input_->points[(*indices_)[i_point]].y;
488 vector (2) = mean_value (2) - input_->points[(*indices_)[i_point]].z;
490 Eigen::Vector3f product = vector.cross (current_axis);
492 float distance = product (0) * product (0) + product (1) * product (1) + product (2) * product (2);
497 return (point_mass_ * moment_of_inertia);
501 template <
typename Po
intT>
void
504 const float D = - normal_vector.dot (point);
506 unsigned int number_of_points =
static_cast <unsigned int> (indices_->size ());
507 projected_cloud->
points.resize (number_of_points,
PointT ());
509 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
511 const unsigned int index = (*indices_)[i_point];
512 float K = - (D + normal_vector (0) * input_->points[index].x + normal_vector (1) * input_->points[index].y + normal_vector (2) * input_->points[index].z);
514 projected_point.x = input_->points[index].x +
K * normal_vector (0);
515 projected_point.y = input_->points[index].y +
K * normal_vector (1);
516 projected_point.z = input_->points[index].z +
K * normal_vector (2);
517 projected_cloud->
points[i_point] = projected_point;
519 projected_cloud->
width = number_of_points;
520 projected_cloud->
height = 1;
521 projected_cloud->
header = input_->header;
525 template <
typename Po
intT>
float
528 Eigen::Vector3f major_axis (0.0f, 0.0f, 0.0f);
529 Eigen::Vector3f middle_axis (0.0f, 0.0f, 0.0f);
530 Eigen::Vector3f minor_axis (0.0f, 0.0f, 0.0f);
531 float major_value = 0.0f;
532 float middle_value = 0.0f;
533 float minor_value = 0.0f;
534 computeEigenVectors (covariance_matrix, major_axis, middle_axis, minor_axis, major_value, middle_value, minor_value);
536 float major = std::abs (major_axis.dot (normal_vector));
537 float middle = std::abs (middle_axis.dot (normal_vector));
538 float minor = std::abs (minor_axis.dot (normal_vector));
540 float eccentricity = 0.0f;
542 if (major >= middle && major >= minor && middle_value != 0.0f)
543 eccentricity = std::pow (1.0f - (minor_value * minor_value) / (middle_value * middle_value), 0.5f);
545 if (middle >= major && middle >= minor && major_value != 0.0f)
546 eccentricity = std::pow (1.0f - (minor_value * minor_value) / (major_value * major_value), 0.5f);
548 if (minor >= major && minor >= middle && major_value != 0.0f)
549 eccentricity = std::pow (1.0f - (middle_value * middle_value) / (major_value * major_value), 0.5f);
551 return (eccentricity);
555 template <
typename Po
intT>
bool
558 mass_center = mean_value_;
564 template <
typename Po
intT>
void
573 template <
typename Po
intT>
void
577 fake_indices_ =
false;
584 template <
typename Po
intT>
void
587 indices_.reset (
new std::vector<int> (*indices));
588 fake_indices_ =
false;
595 template <
typename Po
intT>
void
598 indices_.reset (
new std::vector<int> (indices->indices));
599 fake_indices_ =
false;
606 template <
typename Po
intT>
void
609 if ((nb_rows > input_->height) || (row_start > input_->height))
611 PCL_ERROR (
"[PCLBase::setIndices] cloud is only %d height", input_->height);
615 if ((nb_cols > input_->width) || (col_start > input_->width))
617 PCL_ERROR (
"[PCLBase::setIndices] cloud is only %d width", input_->width);
621 const std::size_t row_end = row_start + nb_rows;
622 if (row_end > input_->height)
624 PCL_ERROR (
"[PCLBase::setIndices] %d is out of rows range %d", row_end, input_->height);
628 const std::size_t col_end = col_start + nb_cols;
629 if (col_end > input_->width)
631 PCL_ERROR (
"[PCLBase::setIndices] %d is out of columns range %d", col_end, input_->width);
635 indices_.reset (
new std::vector<int>);
636 indices_->reserve (nb_cols * nb_rows);
637 for(std::size_t i = row_start; i < row_end; i++)
638 for(std::size_t j = col_start; j < col_end; j++)
639 indices_->push_back (
static_cast<int> ((i * input_->width) + j));
640 fake_indices_ =
false;
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.
typename PointCloud::ConstPtr PointCloudConstPtr
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.