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;
646 #endif // PCL_MOMENT_OF_INERTIA_ESTIMATION_HPP_