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),
59 moment_of_inertia_ (),
65 obb_position_ (0.0f, 0.0f, 0.0f),
66 obb_rotational_matrix_ ()
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_->size () > 0)
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_->points[(*indices_)[i_point]].x - mean_value_ (0)) * major_axis_ (0) +
239 (input_->points[(*indices_)[i_point]].y - mean_value_ (1)) * major_axis_ (1) +
240 (input_->points[(*indices_)[i_point]].z - mean_value_ (2)) * major_axis_ (2);
241 float y = (input_->points[(*indices_)[i_point]].x - mean_value_ (0)) * middle_axis_ (0) +
242 (input_->points[(*indices_)[i_point]].y - mean_value_ (1)) * middle_axis_ (1) +
243 (input_->points[(*indices_)[i_point]].z - mean_value_ (2)) * middle_axis_ (2);
244 float z = (input_->points[(*indices_)[i_point]].x - mean_value_ (0)) * minor_axis_ (0) +
245 (input_->points[(*indices_)[i_point]].y - mean_value_ (1)) * minor_axis_ (1) +
246 (input_->points[(*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_->points[(*indices_)[i_point]].x;
339 mean_value_ (1) += input_->points[(*indices_)[i_point]].y;
340 mean_value_ (2) += input_->points[(*indices_)[i_point]].z;
342 if (input_->points[(*indices_)[i_point]].x <= aabb_min_point_.x) aabb_min_point_.x = input_->points[(*indices_)[i_point]].x;
343 if (input_->points[(*indices_)[i_point]].y <= aabb_min_point_.y) aabb_min_point_.y = input_->points[(*indices_)[i_point]].y;
344 if (input_->points[(*indices_)[i_point]].z <= aabb_min_point_.z) aabb_min_point_.z = input_->points[(*indices_)[i_point]].z;
346 if (input_->points[(*indices_)[i_point]].x >= aabb_max_point_.x) aabb_max_point_.x = input_->points[(*indices_)[i_point]].x;
347 if (input_->points[(*indices_)[i_point]].y >= aabb_max_point_.y) aabb_max_point_.y = input_->points[(*indices_)[i_point]].y;
348 if (input_->points[(*indices_)[i_point]].z >= aabb_max_point_.z) aabb_max_point_.z = input_->points[(*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_->points[(*indices_)[i_point]].x - mean_value_ (0);
371 current_point (1) = input_->points[(*indices_)[i_point]].y - mean_value_ (1);
372 current_point (2) = input_->points[(*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 unsigned int number_of_points = static_cast <
unsigned int> (cloud->points.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->points[i_point].x - mean_value_ (0);
392 current_point (1) = cloud->points[i_point].y - mean_value_ (1);
393 current_point (2) = cloud->points[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 = cos (angle * rad);
472 const float sine = 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_->points[(*indices_)[i_point]].x;
490 vector (1) = mean_value (1) - input_->points[(*indices_)[i_point]].y;
491 vector (2) = mean_value (2) - input_->points[(*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);
497 moment_of_inertia += distance;
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_->points[index].x + normal_vector (1) * input_->points[index].y + normal_vector (2) * input_->points[index].z);
517 projected_point.x = input_->points[index].x + K * normal_vector (0);
518 projected_point.y = input_->points[index].y + K * normal_vector (1);
519 projected_point.z = input_->points[index].z + K * normal_vector (2);
520 projected_cloud->
points[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 = abs (major_axis.dot (normal_vector));
540 float middle = abs (middle_axis.dot (normal_vector));
541 float minor = abs (minor_axis.dot (normal_vector));
543 float eccentricity = 0.0f;
545 if (major >= middle && major >= minor && middle_value != 0.0f)
546 eccentricity = 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 = 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 = 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
576 template <
typename Po
intT>
void
580 fake_indices_ =
false;
587 template <
typename Po
intT>
void
590 indices_.reset (
new std::vector<int> (*indices));
591 fake_indices_ =
false;
598 template <
typename Po
intT>
void
601 indices_.reset (
new std::vector<int> (indices->indices));
602 fake_indices_ =
false;
609 template <
typename Po
intT>
void
612 if ((nb_rows > input_->height) || (row_start > input_->height))
614 PCL_ERROR (
"[PCLBase::setIndices] cloud is only %d height", input_->height);
618 if ((nb_cols > input_->width) || (col_start > input_->width))
620 PCL_ERROR (
"[PCLBase::setIndices] cloud is only %d width", input_->width);
624 size_t row_end = row_start + nb_rows;
625 if (row_end > input_->height)
627 PCL_ERROR (
"[PCLBase::setIndices] %d is out of rows range %d", row_end, input_->height);
631 size_t col_end = col_start + nb_cols;
632 if (col_end > input_->width)
634 PCL_ERROR (
"[PCLBase::setIndices] %d is out of columns range %d", col_end, input_->width);
638 indices_.reset (
new std::vector<int>);
639 indices_->reserve (nb_cols * nb_rows);
640 for(
size_t i = row_start; i < row_end; i++)
641 for(
size_t j = col_start; j < col_end; j++)
642 indices_->push_back (static_cast<int> ((i * input_->width) + j));
643 fake_indices_ =
false;
649 #endif // PCL_MOMENT_OF_INERTIA_ESTIMATION_HPP_
PointCloud::ConstPtr PointCloudConstPtr
float getAngleStep() const
Returns the angle step.
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.
boost::shared_ptr< std::vector< int > > IndicesPtr
boost::shared_ptr< PointCloud< PointT > > Ptr
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.
uint32_t width
The point cloud width (if organized as an image-structure).
pcl::PCLHeader header
The point cloud header.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
Implements the method for extracting features based on moment of inertia.
boost::shared_ptr< PointIndices const > PointIndicesConstPtr
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.
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.
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.
virtual ~MomentOfInertiaEstimation()
Virtual destructor which frees the memory.
void setNormalizePointMassFlag(bool need_to_normalize)
This method allows to set the normalize_ flag.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
void compute()
This method launches the computation of all features.
boost::shared_ptr< const std::vector< int > > IndicesConstPtr
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.
bool getNormalizePointMassFlag() const
Returns the normalize_ flag.
uint32_t height
The point cloud height (if organized as an image-structure).