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> 44 #include <pcl/features/feature.h> 47 template <
typename Po
intT>
51 point_mass_ (0.0001f),
53 mean_value_ (0.0f, 0.0f, 0.0f),
54 major_axis_ (0.0f, 0.0f, 0.0f),
55 middle_axis_ (0.0f, 0.0f, 0.0f),
56 minor_axis_ (0.0f, 0.0f, 0.0f),
64 obb_position_ (0.0f, 0.0f, 0.0f)
69 template <
typename Po
intT>
72 moment_of_inertia_.clear ();
73 eccentricity_.clear ();
77 template <
typename Po
intT>
void 89 template <
typename Po
intT>
float 96 template <
typename Po
intT>
void 99 normalize_ = need_to_normalize;
105 template <
typename Po
intT>
bool 112 template <
typename Po
intT>
void 115 if (point_mass <= 0.0f)
118 point_mass_ = point_mass;
124 template <
typename Po
intT>
float 127 return (point_mass_);
131 template <
typename Po
intT>
void 134 moment_of_inertia_.clear ();
135 eccentricity_.clear ();
145 if (!indices_->empty ())
146 point_mass_ = 1.0f / static_cast <float> (indices_->size () * indices_->size ());
153 Eigen::Matrix <float, 3, 3> covariance_matrix;
154 covariance_matrix.setZero ();
157 computeEigenVectors (covariance_matrix, major_axis_, middle_axis_, minor_axis_, major_value_, middle_value_, minor_value_);
160 while (theta <= 90.0f)
163 Eigen::Vector3f rotated_vector;
164 rotateVector (major_axis_, middle_axis_, theta, rotated_vector);
165 while (phi <= 360.0f)
167 Eigen::Vector3f current_axis;
168 rotateVector (rotated_vector, minor_axis_, phi, current_axis);
169 current_axis.normalize ();
172 float current_moment_of_inertia = calculateMomentOfInertia (current_axis, mean_value_);
173 moment_of_inertia_.push_back (current_moment_of_inertia);
177 getProjectedCloud (current_axis, mean_value_, projected_cloud);
178 Eigen::Matrix <float, 3, 3> covariance_matrix;
179 covariance_matrix.setZero ();
181 projected_cloud.reset ();
182 float current_eccentricity = computeEccentricity (covariance_matrix, current_axis);
183 eccentricity_.push_back (current_eccentricity);
198 template <
typename Po
intT>
bool 201 min_point = aabb_min_point_;
202 max_point = aabb_max_point_;
208 template <
typename Po
intT>
bool 211 min_point = obb_min_point_;
212 max_point = obb_max_point_;
213 position.x = obb_position_ (0);
214 position.y = obb_position_ (1);
215 position.z = obb_position_ (2);
216 rotational_matrix = obb_rotational_matrix_;
222 template <
typename Po
intT>
void 225 obb_min_point_.x = std::numeric_limits <float>::max ();
226 obb_min_point_.y = std::numeric_limits <float>::max ();
227 obb_min_point_.z = std::numeric_limits <float>::max ();
229 obb_max_point_.x = std::numeric_limits <float>::min ();
230 obb_max_point_.y = std::numeric_limits <float>::min ();
231 obb_max_point_.z = std::numeric_limits <float>::min ();
233 unsigned int number_of_points = static_cast <
unsigned int> (indices_->size ());
234 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
236 float x = ((*input_)[(*indices_)[i_point]].x - mean_value_ (0)) * major_axis_ (0) +
237 ((*input_)[(*indices_)[i_point]].y - mean_value_ (1)) * major_axis_ (1) +
238 ((*input_)[(*indices_)[i_point]].z - mean_value_ (2)) * major_axis_ (2);
239 float y = ((*input_)[(*indices_)[i_point]].x - mean_value_ (0)) * middle_axis_ (0) +
240 ((*input_)[(*indices_)[i_point]].y - mean_value_ (1)) * middle_axis_ (1) +
241 ((*input_)[(*indices_)[i_point]].z - mean_value_ (2)) * middle_axis_ (2);
242 float z = ((*input_)[(*indices_)[i_point]].x - mean_value_ (0)) * minor_axis_ (0) +
243 ((*input_)[(*indices_)[i_point]].y - mean_value_ (1)) * minor_axis_ (1) +
244 ((*input_)[(*indices_)[i_point]].z - mean_value_ (2)) * minor_axis_ (2);
246 if (x <= obb_min_point_.x) obb_min_point_.x = x;
247 if (y <= obb_min_point_.y) obb_min_point_.y = y;
248 if (z <= obb_min_point_.z) obb_min_point_.z = z;
250 if (x >= obb_max_point_.x) obb_max_point_.x = x;
251 if (y >= obb_max_point_.y) obb_max_point_.y = y;
252 if (z >= obb_max_point_.z) obb_max_point_.z = z;
255 obb_rotational_matrix_ << major_axis_ (0), middle_axis_ (0), minor_axis_ (0),
256 major_axis_ (1), middle_axis_ (1), minor_axis_ (1),
257 major_axis_ (2), middle_axis_ (2), minor_axis_ (2);
259 Eigen::Vector3f shift (
260 (obb_max_point_.x + obb_min_point_.x) / 2.0f,
261 (obb_max_point_.y + obb_min_point_.y) / 2.0f,
262 (obb_max_point_.z + obb_min_point_.z) / 2.0f);
264 obb_min_point_.x -= shift (0);
265 obb_min_point_.y -= shift (1);
266 obb_min_point_.z -= shift (2);
268 obb_max_point_.x -= shift (0);
269 obb_max_point_.y -= shift (1);
270 obb_max_point_.z -= shift (2);
272 obb_position_ = mean_value_ + obb_rotational_matrix_ * shift;
276 template <
typename Po
intT>
bool 279 major = major_value_;
280 middle = middle_value_;
281 minor = minor_value_;
287 template <
typename Po
intT>
bool 291 middle = middle_axis_;
298 template <
typename Po
intT>
bool 301 moment_of_inertia.resize (moment_of_inertia_.size (), 0.0f);
302 std::copy (moment_of_inertia_.begin (), moment_of_inertia_.end (), moment_of_inertia.begin ());
308 template <
typename Po
intT>
bool 311 eccentricity.resize (eccentricity_.size (), 0.0f);
312 std::copy (eccentricity_.begin (), eccentricity_.end (), eccentricity.begin ());
318 template <
typename Po
intT>
void 321 mean_value_ (0) = 0.0f;
322 mean_value_ (1) = 0.0f;
323 mean_value_ (2) = 0.0f;
325 aabb_min_point_.x = std::numeric_limits <float>::max ();
326 aabb_min_point_.y = std::numeric_limits <float>::max ();
327 aabb_min_point_.z = std::numeric_limits <float>::max ();
329 aabb_max_point_.x = -std::numeric_limits <float>::max ();
330 aabb_max_point_.y = -std::numeric_limits <float>::max ();
331 aabb_max_point_.z = -std::numeric_limits <float>::max ();
333 unsigned int number_of_points = static_cast <
unsigned int> (indices_->size ());
334 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
336 mean_value_ (0) += (*input_)[(*indices_)[i_point]].x;
337 mean_value_ (1) += (*input_)[(*indices_)[i_point]].y;
338 mean_value_ (2) += (*input_)[(*indices_)[i_point]].z;
340 if ((*input_)[(*indices_)[i_point]].x <= aabb_min_point_.x) aabb_min_point_.x = (*input_)[(*indices_)[i_point]].x;
341 if ((*input_)[(*indices_)[i_point]].y <= aabb_min_point_.y) aabb_min_point_.y = (*input_)[(*indices_)[i_point]].y;
342 if ((*input_)[(*indices_)[i_point]].z <= aabb_min_point_.z) aabb_min_point_.z = (*input_)[(*indices_)[i_point]].z;
344 if ((*input_)[(*indices_)[i_point]].x >= aabb_max_point_.x) aabb_max_point_.x = (*input_)[(*indices_)[i_point]].x;
345 if ((*input_)[(*indices_)[i_point]].y >= aabb_max_point_.y) aabb_max_point_.y = (*input_)[(*indices_)[i_point]].y;
346 if ((*input_)[(*indices_)[i_point]].z >= aabb_max_point_.z) aabb_max_point_.z = (*input_)[(*indices_)[i_point]].z;
349 if (number_of_points == 0)
350 number_of_points = 1;
352 mean_value_ (0) /= number_of_points;
353 mean_value_ (1) /= number_of_points;
354 mean_value_ (2) /= number_of_points;
358 template <
typename Po
intT>
void 361 covariance_matrix.setZero ();
363 unsigned int number_of_points = static_cast <
unsigned int> (indices_->size ());
364 float factor = 1.0f / static_cast <
float> ((number_of_points - 1 > 0)?(number_of_points - 1):1);
365 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
367 Eigen::Vector3f current_point (0.0f, 0.0f, 0.0f);
368 current_point (0) = (*input_)[(*indices_)[i_point]].x - mean_value_ (0);
369 current_point (1) = (*input_)[(*indices_)[i_point]].y - mean_value_ (1);
370 current_point (2) = (*input_)[(*indices_)[i_point]].z - mean_value_ (2);
372 covariance_matrix += current_point * current_point.transpose ();
375 covariance_matrix *= factor;
379 template <
typename Po
intT>
void 382 covariance_matrix.setZero ();
384 const auto number_of_points = cloud->size ();
385 float factor = 1.0f / static_cast <
float> ((number_of_points - 1 > 0)?(number_of_points - 1):1);
386 Eigen::Vector3f current_point;
387 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
389 current_point (0) = (*cloud)[i_point].x - mean_value_ (0);
390 current_point (1) = (*cloud)[i_point].y - mean_value_ (1);
391 current_point (2) = (*cloud)[i_point].z - mean_value_ (2);
393 covariance_matrix += current_point * current_point.transpose ();
396 covariance_matrix *= factor;
400 template <
typename Po
intT>
void 402 Eigen::Vector3f& major_axis, Eigen::Vector3f& middle_axis, Eigen::Vector3f& minor_axis,
float& major_value,
403 float& middle_value,
float& minor_value)
405 Eigen::EigenSolver <Eigen::Matrix <float, 3, 3> > eigen_solver;
406 eigen_solver.
compute (covariance_matrix);
408 Eigen::EigenSolver <Eigen::Matrix <float, 3, 3> >::EigenvectorsType eigen_vectors;
409 Eigen::EigenSolver <Eigen::Matrix <float, 3, 3> >::EigenvalueType eigen_values;
410 eigen_vectors = eigen_solver.eigenvectors ();
411 eigen_values = eigen_solver.eigenvalues ();
413 unsigned int temp = 0;
414 unsigned int major_index = 0;
415 unsigned int middle_index = 1;
416 unsigned int minor_index = 2;
418 if (eigen_values.real () (major_index) < eigen_values.real () (middle_index))
421 major_index = middle_index;
425 if (eigen_values.real () (major_index) < eigen_values.real () (minor_index))
428 major_index = minor_index;
432 if (eigen_values.real () (middle_index) < eigen_values.real () (minor_index))
435 minor_index = middle_index;
439 major_value = eigen_values.real () (major_index);
440 middle_value = eigen_values.real () (middle_index);
441 minor_value = eigen_values.real () (minor_index);
443 major_axis = eigen_vectors.col (major_index).real ();
444 middle_axis = eigen_vectors.col (middle_index).real ();
445 minor_axis = eigen_vectors.col (minor_index).real ();
447 major_axis.normalize ();
448 middle_axis.normalize ();
449 minor_axis.normalize ();
451 float det = major_axis.dot (middle_axis.cross (minor_axis));
454 major_axis (0) = -major_axis (0);
455 major_axis (1) = -major_axis (1);
456 major_axis (2) = -major_axis (2);
461 template <
typename Po
intT>
void 464 Eigen::Matrix <float, 3, 3> rotation_matrix;
465 const float x = axis (0);
466 const float y = axis (1);
467 const float z = axis (2);
468 const float rad =
M_PI / 180.0f;
469 const float cosine = std::cos (angle * rad);
470 const float sine = std::sin (angle * rad);
471 rotation_matrix << cosine + (1 - cosine) * x * x, (1 - cosine) * x * y - sine * z, (1 - cosine) * x * z + sine * y,
472 (1 - cosine) * y * x + sine * z, cosine + (1 - cosine) * y * y, (1 - cosine) * y * z - sine * x,
473 (1 - cosine) * z * x - sine * y, (1 - cosine) * z * y + sine * x, cosine + (1 - cosine) * z * z;
475 rotated_vector = rotation_matrix * vector;
479 template <
typename Po
intT>
float 482 float moment_of_inertia = 0.0f;
483 unsigned int number_of_points = static_cast <
unsigned int> (indices_->size ());
484 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
486 Eigen::Vector3f vector;
487 vector (0) = mean_value (0) - (*input_)[(*indices_)[i_point]].x;
488 vector (1) = mean_value (1) - (*input_)[(*indices_)[i_point]].y;
489 vector (2) = mean_value (2) - (*input_)[(*indices_)[i_point]].z;
491 Eigen::Vector3f product = vector.cross (current_axis);
493 float distance = product (0) * product (0) + product (1) * product (1) + product (2) * product (2);
498 return (point_mass_ * moment_of_inertia);
502 template <
typename Po
intT>
void 505 const float D = - normal_vector.dot (point);
507 unsigned int number_of_points = static_cast <
unsigned int> (indices_->size ());
508 projected_cloud->
points.resize (number_of_points,
PointT ());
510 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
512 const unsigned int index = (*indices_)[i_point];
513 float K = - (D + normal_vector (0) * (*input_)[index].x + normal_vector (1) * (*input_)[index].y + normal_vector (2) * (*input_)[index].z);
515 projected_point.x = (*input_)[index].x +
K * normal_vector (0);
516 projected_point.y = (*input_)[index].y +
K * normal_vector (1);
517 projected_point.z = (*input_)[index].z +
K * normal_vector (2);
518 (*projected_cloud)[i_point] = projected_point;
520 projected_cloud->
width = number_of_points;
521 projected_cloud->
height = 1;
522 projected_cloud->
header = input_->header;
526 template <
typename Po
intT>
float 529 Eigen::Vector3f major_axis (0.0f, 0.0f, 0.0f);
530 Eigen::Vector3f middle_axis (0.0f, 0.0f, 0.0f);
531 Eigen::Vector3f minor_axis (0.0f, 0.0f, 0.0f);
532 float major_value = 0.0f;
533 float middle_value = 0.0f;
534 float minor_value = 0.0f;
535 computeEigenVectors (covariance_matrix, major_axis, middle_axis, minor_axis, major_value, middle_value, minor_value);
537 float major = std::abs (major_axis.dot (normal_vector));
538 float middle = std::abs (middle_axis.dot (normal_vector));
539 float minor = std::abs (minor_axis.dot (normal_vector));
541 float eccentricity = 0.0f;
543 if (major >= middle && major >= minor && middle_value != 0.0f)
544 eccentricity = std::pow (1.0f - (minor_value * minor_value) / (middle_value * middle_value), 0.5f);
546 if (middle >= major && middle >= minor && major_value != 0.0f)
547 eccentricity = std::pow (1.0f - (minor_value * minor_value) / (major_value * major_value), 0.5f);
549 if (minor >= major && minor >= middle && major_value != 0.0f)
550 eccentricity = std::pow (1.0f - (middle_value * middle_value) / (major_value * major_value), 0.5f);
552 return (eccentricity);
556 template <
typename Po
intT>
bool 559 mass_center = mean_value_;
565 template <
typename Po
intT>
void 574 template <
typename Po
intT>
void 578 fake_indices_ =
false;
585 template <
typename Po
intT>
void 588 indices_.reset (
new std::vector<int> (*indices));
589 fake_indices_ =
false;
596 template <
typename Po
intT>
void 599 indices_.reset (
new std::vector<int> (indices->indices));
600 fake_indices_ =
false;
607 template <
typename Po
intT>
void 610 if ((nb_rows > input_->height) || (row_start > input_->height))
612 PCL_ERROR (
"[PCLBase::setIndices] cloud is only %d height", input_->height);
616 if ((nb_cols > input_->width) || (col_start > input_->width))
618 PCL_ERROR (
"[PCLBase::setIndices] cloud is only %d width", input_->width);
622 const std::size_t row_end = row_start + nb_rows;
623 if (row_end > input_->height)
625 PCL_ERROR (
"[PCLBase::setIndices] %d is out of rows range %d", row_end, input_->height);
629 const std::size_t col_end = col_start + nb_cols;
630 if (col_end > input_->width)
632 PCL_ERROR (
"[PCLBase::setIndices] %d is out of columns range %d", col_end, input_->width);
636 indices_.reset (
new std::vector<int>);
637 indices_->reserve (nb_cols * nb_rows);
638 for(std::size_t i = row_start; i < row_end; i++)
639 for(std::size_t j = col_start; j < col_end; j++)
640 indices_->push_back (static_cast<int> ((i * input_->width) + j));
641 fake_indices_ =
false;
647 #endif // PCL_MOMENT_OF_INERTIA_ESTIMATION_HPP_ 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 getAABB(PointT &min_point, PointT &max_point) const
This method gives access to the computed axis aligned bounding box.
shared_ptr< PointCloud< PointT > > Ptr
void setIndices(const IndicesPtr &indices) override
Provide a pointer to the vector of indices that represents the input data.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
float getPointMass() const
Returns the mass of point.
shared_ptr< Indices > IndicesPtr
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.
bool getNormalizePointMassFlag() const
Returns the normalize_ flag.
Implements the method for extracting features based on moment of inertia.
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.
bool getMassCenter(Eigen::Vector3f &mass_center) const
This method gives access to the computed mass center.
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.
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.
pcl::PCLHeader header
The point cloud header.
~MomentOfInertiaEstimation()
Virtual destructor which frees the memory.
void setNormalizePointMassFlag(bool need_to_normalize)
This method allows to set the normalize_ flag.
bool getEigenValues(float &major, float &middle, float &minor) const
This method gives access to the computed eigen values.
void compute()
This method launches the computation of all features.
float getAngleStep() const
Returns the angle step.
void setAngleStep(const float step)
This method allows to set the angle step.
A point structure representing Euclidean xyz coordinates, and the RGB color.
PointIndices::ConstPtr PointIndicesConstPtr
bool getMomentOfInertia(std::vector< float > &moment_of_inertia) const
This method gives access to the computed moments of inertia.
typename PointCloud::ConstPtr PointCloudConstPtr