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 ();
148 point_mass_ = 1.0f / static_cast <float> (
indices_->size () *
indices_->size ());
155 Eigen::Matrix <float, 3, 3> covariance_matrix;
156 covariance_matrix.setZero ();
157 computeCovarianceMatrix (covariance_matrix);
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 ();
182 computeCovarianceMatrix (projected_cloud, covariance_matrix);
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;
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 = std::abs (major_axis.dot (normal_vector));
540 float middle = std::abs (middle_axis.dot (normal_vector));
541 float minor = std::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 587 template <
typename Po
intT>
void 590 indices_.reset (
new std::vector<int> (*indices));
598 template <
typename Po
intT>
void 601 indices_.reset (
new std::vector<int> (indices->indices));
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++)
649 #endif // PCL_MOMENT_OF_INERTIA_ESTIMATION_HPP_ PointCloud::ConstPtr PointCloudConstPtr
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.
boost::shared_ptr< std::vector< int > > IndicesPtr
bool getEigenVectors(Eigen::Vector3f &major, Eigen::Vector3f &middle, Eigen::Vector3f &minor) const
This method gives access to the computed eigen vectors.
IndicesPtr indices_
A pointer to the vector of point indices to use.
bool getEccentricity(std::vector< float > &eccentricity) const
This method gives access to the computed ecentricities.
bool use_indices_
Set to true if point indices are used.
uint32_t height
The point cloud height (if organized as an image-structure).
bool initCompute()
This method should get called before starting the actual computation.
boost::shared_ptr< PointCloud< PointT > > Ptr
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
uint32_t width
The point cloud width (if organized as an image-structure).
Implements the method for extracting features based on moment of inertia.
boost::shared_ptr< PointIndices const > PointIndicesConstPtr
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.
bool deinitCompute()
This method should get called after finishing the actual computation.
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 ~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
PointCloudConstPtr input_
The input point cloud 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.
bool fake_indices_
If no set of indices are given, we construct a set of fake indices that mimic the input PointCloud...
bool getNormalizePointMassFlag() const
Returns the normalize_ flag.