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_