43 #include <pcl/common/centroid.h>
44 #include <pcl/conversions.h>
45 #include <pcl/common/point_tests.h>
46 #include <Eigen/Eigenvalues>
48 #include <boost/fusion/algorithm/transformation/filter_if.hpp>
49 #include <boost/fusion/algorithm/iteration/for_each.hpp>
50 #include <boost/mpl/size.hpp>
56 template <
typename Po
intT,
typename Scalar>
inline unsigned int
58 Eigen::Matrix<Scalar, 4, 1> ¢roid)
60 Eigen::Matrix<Scalar, 4, 1> accumulator {0, 0, 0, 0};
66 while (cloud_iterator.
isValid ())
71 accumulator[0] += cloud_iterator->x;
72 accumulator[1] += cloud_iterator->y;
73 accumulator[2] += cloud_iterator->z;
80 centroid = accumulator;
81 centroid /=
static_cast<Scalar
> (cp);
88 template <
typename Po
intT,
typename Scalar>
inline unsigned int
90 Eigen::Matrix<Scalar, 4, 1> ¢roid)
101 for (
const auto& point: cloud)
103 centroid[0] += point.x;
104 centroid[1] += point.y;
105 centroid[2] += point.z;
107 centroid /=
static_cast<Scalar
> (cloud.size ());
110 return (static_cast<unsigned int> (cloud.size ()));
114 Eigen::Matrix<Scalar, 4, 1> accumulator {0, 0, 0, 0};
115 for (
const auto& point: cloud)
121 accumulator[0] += point.x;
122 accumulator[1] += point.y;
123 accumulator[2] += point.z;
127 centroid = accumulator;
128 centroid /=
static_cast<Scalar
> (cp);
136 template <
typename Po
intT,
typename Scalar>
inline unsigned int
139 Eigen::Matrix<Scalar, 4, 1> ¢roid)
141 if (indices.empty ())
149 for (
const auto& index : indices)
151 centroid[0] += cloud[index].x;
152 centroid[1] += cloud[index].y;
153 centroid[2] += cloud[index].z;
155 centroid /=
static_cast<Scalar
> (indices.size ());
157 return (static_cast<unsigned int> (indices.size ()));
160 Eigen::Matrix<Scalar, 4, 1> accumulator {0, 0, 0, 0};
162 for (
const auto& index : indices)
168 accumulator[0] += cloud[index].x;
169 accumulator[1] += cloud[index].y;
170 accumulator[2] += cloud[index].z;
174 centroid = accumulator;
175 centroid /=
static_cast<Scalar
> (cp);
182 template <
typename Po
intT,
typename Scalar>
inline unsigned int
185 Eigen::Matrix<Scalar, 4, 1> ¢roid)
191 template <
typename Po
intT,
typename Scalar>
inline unsigned
193 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
194 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
199 unsigned point_count;
203 covariance_matrix.setZero ();
204 point_count =
static_cast<unsigned> (cloud.
size ());
206 for (
const auto& point: cloud)
208 Eigen::Matrix<Scalar, 4, 1> pt;
209 pt[0] = point.x - centroid[0];
210 pt[1] = point.y - centroid[1];
211 pt[2] = point.z - centroid[2];
213 covariance_matrix (1, 1) += pt.y () * pt.y ();
214 covariance_matrix (1, 2) += pt.y () * pt.z ();
216 covariance_matrix (2, 2) += pt.z () * pt.z ();
219 covariance_matrix (0, 0) += pt.x ();
220 covariance_matrix (0, 1) += pt.y ();
221 covariance_matrix (0, 2) += pt.z ();
227 Eigen::Matrix<Scalar, 3, 3> temp_covariance_matrix;
228 temp_covariance_matrix.setZero();
231 for (
const auto& point: cloud)
237 Eigen::Matrix<Scalar, 4, 1> pt;
238 pt[0] = point.x - centroid[0];
239 pt[1] = point.y - centroid[1];
240 pt[2] = point.z - centroid[2];
242 temp_covariance_matrix (1, 1) += pt.y () * pt.y ();
243 temp_covariance_matrix (1, 2) += pt.y () * pt.z ();
245 temp_covariance_matrix (2, 2) += pt.z () * pt.z ();
248 temp_covariance_matrix (0, 0) += pt.x ();
249 temp_covariance_matrix (0, 1) += pt.y ();
250 temp_covariance_matrix (0, 2) += pt.z ();
253 if (point_count > 0) {
254 covariance_matrix = temp_covariance_matrix;
257 if (point_count == 0) {
260 covariance_matrix (1, 0) = covariance_matrix (0, 1);
261 covariance_matrix (2, 0) = covariance_matrix (0, 2);
262 covariance_matrix (2, 1) = covariance_matrix (1, 2);
264 return (point_count);
268 template <
typename Po
intT,
typename Scalar>
inline unsigned int
270 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
271 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
274 if (point_count != 0)
275 covariance_matrix /=
static_cast<Scalar
> (point_count);
276 return (point_count);
280 template <
typename Po
intT,
typename Scalar>
inline unsigned int
283 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
284 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
286 if (indices.empty ())
289 std::size_t point_count;
293 covariance_matrix.setZero ();
294 point_count = indices.size ();
296 for (
const auto& idx: indices)
298 Eigen::Matrix<Scalar, 4, 1> pt;
299 pt[0] = cloud[idx].x - centroid[0];
300 pt[1] = cloud[idx].y - centroid[1];
301 pt[2] = cloud[idx].z - centroid[2];
303 covariance_matrix (1, 1) += pt.y () * pt.y ();
304 covariance_matrix (1, 2) += pt.y () * pt.z ();
306 covariance_matrix (2, 2) += pt.z () * pt.z ();
309 covariance_matrix (0, 0) += pt.x ();
310 covariance_matrix (0, 1) += pt.y ();
311 covariance_matrix (0, 2) += pt.z ();
317 Eigen::Matrix<Scalar, 3, 3> temp_covariance_matrix;
318 temp_covariance_matrix.setZero ();
321 for (
const auto &index : indices)
327 Eigen::Matrix<Scalar, 4, 1> pt;
328 pt[0] = cloud[index].x - centroid[0];
329 pt[1] = cloud[index].y - centroid[1];
330 pt[2] = cloud[index].z - centroid[2];
332 temp_covariance_matrix (1, 1) += pt.y () * pt.y ();
333 temp_covariance_matrix (1, 2) += pt.y () * pt.z ();
335 temp_covariance_matrix (2, 2) += pt.z () * pt.z ();
338 temp_covariance_matrix (0, 0) += pt.x ();
339 temp_covariance_matrix (0, 1) += pt.y ();
340 temp_covariance_matrix (0, 2) += pt.z ();
343 if (point_count > 0) {
344 covariance_matrix = temp_covariance_matrix;
347 if (point_count == 0) {
350 covariance_matrix (1, 0) = covariance_matrix (0, 1);
351 covariance_matrix (2, 0) = covariance_matrix (0, 2);
352 covariance_matrix (2, 1) = covariance_matrix (1, 2);
353 return (static_cast<unsigned int> (point_count));
357 template <
typename Po
intT,
typename Scalar>
inline unsigned int
360 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
361 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
367 template <
typename Po
intT,
typename Scalar>
inline unsigned int
370 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
371 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
374 if (point_count != 0)
375 covariance_matrix /=
static_cast<Scalar
> (point_count);
377 return (point_count);
381 template <
typename Po
intT,
typename Scalar>
inline unsigned int
384 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
385 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
391 template <
typename Po
intT,
typename Scalar>
inline unsigned int
393 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
396 Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero ();
398 unsigned int point_count;
401 point_count =
static_cast<unsigned int> (cloud.
size ());
403 for (
const auto& point: cloud)
405 accu [0] += point.x * point.x;
406 accu [1] += point.x * point.y;
407 accu [2] += point.x * point.z;
408 accu [3] += point.y * point.y;
409 accu [4] += point.y * point.z;
410 accu [5] += point.z * point.z;
416 for (
const auto& point: cloud)
421 accu [0] += point.x * point.x;
422 accu [1] += point.x * point.y;
423 accu [2] += point.x * point.z;
424 accu [3] += point.y * point.y;
425 accu [4] += point.y * point.z;
426 accu [5] += point.z * point.z;
431 if (point_count != 0)
433 accu /=
static_cast<Scalar
> (point_count);
434 covariance_matrix.coeffRef (0) = accu [0];
435 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
436 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
437 covariance_matrix.coeffRef (4) = accu [3];
438 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
439 covariance_matrix.coeffRef (8) = accu [5];
441 return (point_count);
445 template <
typename Po
intT,
typename Scalar>
inline unsigned int
448 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
451 Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero ();
453 unsigned int point_count;
456 point_count =
static_cast<unsigned int> (indices.size ());
457 for (
const auto &index : indices)
460 accu [0] += cloud[index].x * cloud[index].x;
461 accu [1] += cloud[index].x * cloud[index].y;
462 accu [2] += cloud[index].x * cloud[index].z;
463 accu [3] += cloud[index].y * cloud[index].y;
464 accu [4] += cloud[index].y * cloud[index].z;
465 accu [5] += cloud[index].z * cloud[index].z;
471 for (
const auto &index : indices)
477 accu [0] += cloud[index].x * cloud[index].x;
478 accu [1] += cloud[index].x * cloud[index].y;
479 accu [2] += cloud[index].x * cloud[index].z;
480 accu [3] += cloud[index].y * cloud[index].y;
481 accu [4] += cloud[index].y * cloud[index].z;
482 accu [5] += cloud[index].z * cloud[index].z;
485 if (point_count != 0)
487 accu /=
static_cast<Scalar
> (point_count);
488 covariance_matrix.coeffRef (0) = accu [0];
489 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
490 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
491 covariance_matrix.coeffRef (4) = accu [3];
492 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
493 covariance_matrix.coeffRef (8) = accu [5];
495 return (point_count);
499 template <
typename Po
intT,
typename Scalar>
inline unsigned int
502 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
508 template <
typename Po
intT,
typename Scalar>
inline unsigned int
510 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
511 Eigen::Matrix<Scalar, 4, 1> ¢roid)
515 Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
516 Eigen::Matrix<Scalar, 3, 1>
K(0.0, 0.0, 0.0);
517 for(
const auto& point: cloud)
519 K.x() = point.x; K.y() = point.y; K.z() = point.z;
break;
521 std::size_t point_count;
524 point_count = cloud.size ();
526 for (
const auto& point: cloud)
528 Scalar x = point.x - K.x(), y = point.y - K.y(), z = point.z - K.z();
543 for (
const auto& point: cloud)
548 Scalar x = point.x - K.x(), y = point.y - K.y(), z = point.z - K.z();
561 if (point_count != 0)
563 accu /=
static_cast<Scalar
> (point_count);
564 centroid[0] = accu[6] + K.x(); centroid[1] = accu[7] + K.y(); centroid[2] = accu[8] + K.z();
566 covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
567 covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
568 covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
569 covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
570 covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
571 covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
572 covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
573 covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
574 covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
576 return (static_cast<unsigned int> (point_count));
580 template <
typename Po
intT,
typename Scalar>
inline unsigned int
583 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
584 Eigen::Matrix<Scalar, 4, 1> ¢roid)
588 Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
589 Eigen::Matrix<Scalar, 3, 1>
K(0.0, 0.0, 0.0);
590 for(
const auto& index : indices)
592 K.x() = cloud[index].x; K.y() = cloud[index].y; K.z() = cloud[index].z;
break;
594 std::size_t point_count;
597 point_count = indices.
size ();
598 for (
const auto &index : indices)
600 Scalar x = cloud[index].x - K.x(), y = cloud[index].y - K.y(), z = cloud[index].z - K.z();
615 for (
const auto &index : indices)
621 Scalar x = cloud[index].x - K.x(), y = cloud[index].y - K.y(), z = cloud[index].z - K.z();
634 if (point_count != 0)
636 accu /=
static_cast<Scalar
> (point_count);
637 centroid[0] = accu[6] + K.x(); centroid[1] = accu[7] + K.y(); centroid[2] = accu[8] + K.z();
639 covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
640 covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
641 covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
642 covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
643 covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
644 covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
645 covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
646 covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
647 covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
649 return (static_cast<unsigned int> (point_count));
653 template <
typename Po
intT,
typename Scalar>
inline unsigned int
656 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
657 Eigen::Matrix<Scalar, 4, 1> ¢roid)
662 template <
typename Po
intT,
typename Scalar>
inline unsigned int
664 Eigen::Matrix<Scalar, 3, 1> ¢roid,
665 Eigen::Matrix<Scalar, 3, 1> &obb_center,
666 Eigen::Matrix<Scalar, 3, 1> &obb_dimensions,
667 Eigen::Matrix<Scalar, 3, 3> &obb_rotational_matrix)
669 Eigen::Matrix<Scalar, 3, 3> covariance_matrix;
670 Eigen::Matrix<Scalar, 4, 1> centroid4;
674 centroid(0) = centroid4(0);
675 centroid(1) = centroid4(1);
676 centroid(2) = centroid4(2);
678 const Eigen::SelfAdjointEigenSolver<Eigen::Matrix<Scalar, 3, 3>> evd(covariance_matrix);
679 const Eigen::Matrix<Scalar, 3, 3> eigenvectors_ = evd.eigenvectors();
680 const Eigen::Matrix<Scalar, 3, 1> minor_axis = eigenvectors_.col(0);
681 const Eigen::Matrix<Scalar, 3, 1> middle_axis = eigenvectors_.col(1);
683 const Eigen::Matrix<Scalar, 3, 1> major_axis = middle_axis.cross(minor_axis);
685 obb_rotational_matrix <<
686 major_axis(0), middle_axis(0), minor_axis(0),
687 major_axis(1), middle_axis(1), minor_axis(1),
688 major_axis(2), middle_axis(2), minor_axis(2);
697 Eigen::Matrix<Scalar, 4, 4> transform = Eigen::Matrix<Scalar, 4, 4>::Identity();
698 transform.template topLeftCorner<3, 3>() = obb_rotational_matrix.transpose();
699 transform.template topRightCorner<3, 1>() =-transform.template topLeftCorner<3, 3>()*centroid;
703 Scalar obb_min_pointx, obb_min_pointy, obb_min_pointz;
704 Scalar obb_max_pointx, obb_max_pointy, obb_max_pointz;
705 obb_min_pointx = obb_min_pointy = obb_min_pointz = std::numeric_limits<Scalar>::max();
706 obb_max_pointx = obb_max_pointy = obb_max_pointz = std::numeric_limits<Scalar>::min();
710 const auto& point = cloud[0];
711 Eigen::Matrix<Scalar, 4, 1> P0(static_cast<Scalar>(point.x), static_cast<Scalar>(point.y) , static_cast<Scalar>(point.z), 1.0);
712 Eigen::Matrix<Scalar, 4, 1> P = transform * P0;
714 obb_min_pointx = obb_max_pointx = P(0);
715 obb_min_pointy = obb_max_pointy = P(1);
716 obb_min_pointz = obb_max_pointz = P(2);
718 for (
size_t i=1; i<cloud.
size();++i)
720 const auto& point = cloud[i];
721 Eigen::Matrix<Scalar, 4, 1> P0(static_cast<Scalar>(point.x), static_cast<Scalar>(point.y) , static_cast<Scalar>(point.z), 1.0);
722 Eigen::Matrix<Scalar, 4, 1> P = transform * P0;
724 if (P(0) < obb_min_pointx)
725 obb_min_pointx = P(0);
726 else if (P(0) > obb_max_pointx)
727 obb_max_pointx = P(0);
728 if (P(1) < obb_min_pointy)
729 obb_min_pointy = P(1);
730 else if (P(1) > obb_max_pointy)
731 obb_max_pointy = P(1);
732 if (P(2) < obb_min_pointz)
733 obb_min_pointz = P(2);
734 else if (P(2) > obb_max_pointz)
735 obb_max_pointz = P(2);
741 for (; i < cloud.
size(); ++i)
743 const auto& point = cloud[i];
746 Eigen::Matrix<Scalar, 4, 1> P0(static_cast<Scalar>(point.x), static_cast<Scalar>(point.y) , static_cast<Scalar>(point.z), 1.0);
747 Eigen::Matrix<Scalar, 4, 1> P = transform * P0;
749 obb_min_pointx = obb_max_pointx = P(0);
750 obb_min_pointy = obb_max_pointy = P(1);
751 obb_min_pointz = obb_max_pointz = P(2);
756 for (; i<cloud.
size();++i)
758 const auto& point = cloud[i];
761 Eigen::Matrix<Scalar, 4, 1> P0(static_cast<Scalar>(point.x), static_cast<Scalar>(point.y) , static_cast<Scalar>(point.z), 1.0);
762 Eigen::Matrix<Scalar, 4, 1> P = transform * P0;
764 if (P(0) < obb_min_pointx)
765 obb_min_pointx = P(0);
766 else if (P(0) > obb_max_pointx)
767 obb_max_pointx = P(0);
768 if (P(1) < obb_min_pointy)
769 obb_min_pointy = P(1);
770 else if (P(1) > obb_max_pointy)
771 obb_max_pointy = P(1);
772 if (P(2) < obb_min_pointz)
773 obb_min_pointz = P(2);
774 else if (P(2) > obb_max_pointz)
775 obb_max_pointz = P(2);
780 const Eigen::Matrix<Scalar, 3, 1>
781 shift((obb_max_pointx + obb_min_pointx) / 2.0f,
782 (obb_max_pointy + obb_min_pointy) / 2.0f,
783 (obb_max_pointz + obb_min_pointz) / 2.0f);
785 obb_dimensions(0) = obb_max_pointx - obb_min_pointx;
786 obb_dimensions(1) = obb_max_pointy - obb_min_pointy;
787 obb_dimensions(2) = obb_max_pointz - obb_min_pointz;
789 obb_center = centroid + obb_rotational_matrix * shift;
791 return (point_count);
795 template <
typename Po
intT,
typename Scalar>
inline unsigned int
798 Eigen::Matrix<Scalar, 3, 1> ¢roid,
799 Eigen::Matrix<Scalar, 3, 1> &obb_center,
800 Eigen::Matrix<Scalar, 3, 1> &obb_dimensions,
801 Eigen::Matrix<Scalar, 3, 3> &obb_rotational_matrix)
803 Eigen::Matrix<Scalar, 3, 3> covariance_matrix;
804 Eigen::Matrix<Scalar, 4, 1> centroid4;
808 centroid(0) = centroid4(0);
809 centroid(1) = centroid4(1);
810 centroid(2) = centroid4(2);
812 const Eigen::SelfAdjointEigenSolver<Eigen::Matrix<Scalar, 3, 3>> evd(covariance_matrix);
813 const Eigen::Matrix<Scalar, 3, 3> eigenvectors_ = evd.eigenvectors();
814 const Eigen::Matrix<Scalar, 3, 1> minor_axis = eigenvectors_.col(0);
815 const Eigen::Matrix<Scalar, 3, 1> middle_axis = eigenvectors_.col(1);
817 const Eigen::Matrix<Scalar, 3, 1> major_axis = middle_axis.cross(minor_axis);
819 obb_rotational_matrix <<
820 major_axis(0), middle_axis(0), minor_axis(0),
821 major_axis(1), middle_axis(1), minor_axis(1),
822 major_axis(2), middle_axis(2), minor_axis(2);
831 Eigen::Matrix<Scalar, 4, 4> transform = Eigen::Matrix<Scalar, 4, 4>::Identity();
832 transform.template topLeftCorner<3, 3>() = obb_rotational_matrix.transpose();
833 transform.template topRightCorner<3, 1>() =-transform.template topLeftCorner<3, 3>()*centroid;
837 Scalar obb_min_pointx, obb_min_pointy, obb_min_pointz;
838 Scalar obb_max_pointx, obb_max_pointy, obb_max_pointz;
839 obb_min_pointx = obb_min_pointy = obb_min_pointz = std::numeric_limits<Scalar>::max();
840 obb_max_pointx = obb_max_pointy = obb_max_pointz = std::numeric_limits<Scalar>::min();
844 const auto& point = cloud[indices[0]];
845 Eigen::Matrix<Scalar, 4, 1> P0(static_cast<Scalar>(point.x), static_cast<Scalar>(point.y) , static_cast<Scalar>(point.z), 1.0);
846 Eigen::Matrix<Scalar, 4, 1> P = transform * P0;
848 obb_min_pointx = obb_max_pointx = P(0);
849 obb_min_pointy = obb_max_pointy = P(1);
850 obb_min_pointz = obb_max_pointz = P(2);
852 for (
size_t i=1; i<indices.size();++i)
854 const auto & point = cloud[indices[i]];
856 Eigen::Matrix<Scalar, 4, 1> P0(static_cast<Scalar>(point.x), static_cast<Scalar>(point.y) , static_cast<Scalar>(point.z), 1.0);
857 Eigen::Matrix<Scalar, 4, 1> P = transform * P0;
859 if (P(0) < obb_min_pointx)
860 obb_min_pointx = P(0);
861 else if (P(0) > obb_max_pointx)
862 obb_max_pointx = P(0);
863 if (P(1) < obb_min_pointy)
864 obb_min_pointy = P(1);
865 else if (P(1) > obb_max_pointy)
866 obb_max_pointy = P(1);
867 if (P(2) < obb_min_pointz)
868 obb_min_pointz = P(2);
869 else if (P(2) > obb_max_pointz)
870 obb_max_pointz = P(2);
876 for (; i<indices.size();++i)
878 const auto& point = cloud[indices[i]];
881 Eigen::Matrix<Scalar, 4, 1> P0(static_cast<Scalar>(point.x), static_cast<Scalar>(point.y) , static_cast<Scalar>(point.z), 1.0);
882 Eigen::Matrix<Scalar, 4, 1> P = transform * P0;
884 obb_min_pointx = obb_max_pointx = P(0);
885 obb_min_pointy = obb_max_pointy = P(1);
886 obb_min_pointz = obb_max_pointz = P(2);
891 for (; i<indices.size();++i)
893 const auto& point = cloud[indices[i]];
897 Eigen::Matrix<Scalar, 4, 1> P0(static_cast<Scalar>(point.x), static_cast<Scalar>(point.y) , static_cast<Scalar>(point.z), 1.0);
898 Eigen::Matrix<Scalar, 4, 1> P = transform * P0;
900 if (P(0) < obb_min_pointx)
901 obb_min_pointx = P(0);
902 else if (P(0) > obb_max_pointx)
903 obb_max_pointx = P(0);
904 if (P(1) < obb_min_pointy)
905 obb_min_pointy = P(1);
906 else if (P(1) > obb_max_pointy)
907 obb_max_pointy = P(1);
908 if (P(2) < obb_min_pointz)
909 obb_min_pointz = P(2);
910 else if (P(2) > obb_max_pointz)
911 obb_max_pointz = P(2);
916 const Eigen::Matrix<Scalar, 3, 1>
917 shift((obb_max_pointx + obb_min_pointx) / 2.0f,
918 (obb_max_pointy + obb_min_pointy) / 2.0f,
919 (obb_max_pointz + obb_min_pointz) / 2.0f);
921 obb_dimensions(0) = obb_max_pointx - obb_min_pointx;
922 obb_dimensions(1) = obb_max_pointy - obb_min_pointy;
923 obb_dimensions(2) = obb_max_pointz - obb_min_pointz;
925 obb_center = centroid + obb_rotational_matrix * shift;
927 return (point_count);
932 template <
typename Po
intT,
typename Scalar>
void
934 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
941 while (cloud_iterator.
isValid ())
946 cloud_iterator.
reset ();
952 while (cloud_iterator.
isValid ())
954 cloud_out[i].x = cloud_iterator->x - centroid[0];
955 cloud_out[i].y = cloud_iterator->y - centroid[1];
956 cloud_out[i].z = cloud_iterator->z - centroid[2];
965 template <
typename Po
intT,
typename Scalar>
void
967 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
970 cloud_out = cloud_in;
973 for (
auto& point: cloud_out)
975 point.x -=
static_cast<float> (centroid[0]);
976 point.y -=
static_cast<float> (centroid[1]);
977 point.z -=
static_cast<float> (centroid[2]);
982 template <
typename Po
intT,
typename Scalar>
void
985 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
990 if (indices.size () == cloud_in.
size ())
997 cloud_out.
width = indices.size ();
1000 cloud_out.
resize (indices.size ());
1003 for (std::size_t i = 0; i < indices.size (); ++i)
1005 cloud_out[i].x =
static_cast<float> (cloud_in[indices[i]].x - centroid[0]);
1006 cloud_out[i].y =
static_cast<float> (cloud_in[indices[i]].y - centroid[1]);
1007 cloud_out[i].z =
static_cast<float> (cloud_in[indices[i]].z - centroid[2]);
1012 template <
typename Po
intT,
typename Scalar>
void
1015 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
1022 template <
typename Po
intT,
typename Scalar>
void
1024 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
1025 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out,
1031 while (cloud_iterator.
isValid ())
1036 cloud_iterator.
reset ();
1039 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts);
1042 while (cloud_iterator.
isValid ())
1044 cloud_out (0, i) = cloud_iterator->x - centroid[0];
1045 cloud_out (1, i) = cloud_iterator->y - centroid[1];
1046 cloud_out (2, i) = cloud_iterator->z - centroid[2];
1053 template <
typename Po
intT,
typename Scalar>
void
1055 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
1056 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
1058 std::size_t npts = cloud_in.
size ();
1060 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts);
1062 for (std::size_t i = 0; i < npts; ++i)
1064 cloud_out (0, i) = cloud_in[i].x - centroid[0];
1065 cloud_out (1, i) = cloud_in[i].y - centroid[1];
1066 cloud_out (2, i) = cloud_in[i].z - centroid[2];
1076 template <
typename Po
intT,
typename Scalar>
void
1079 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
1080 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
1082 std::size_t npts = indices.size ();
1084 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts);
1086 for (std::size_t i = 0; i < npts; ++i)
1088 cloud_out (0, i) = cloud_in[indices[i]].x - centroid[0];
1089 cloud_out (1, i) = cloud_in[indices[i]].y - centroid[1];
1090 cloud_out (2, i) = cloud_in[indices[i]].z - centroid[2];
1100 template <
typename Po
intT,
typename Scalar>
void
1103 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
1104 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
1110 template <
typename Po
intT,
typename Scalar>
inline void
1112 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid)
1114 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
1117 centroid.setZero (boost::mpl::size<FieldList>::value);
1123 for (
const auto& pt: cloud)
1128 centroid /=
static_cast<Scalar
> (cloud.size ());
1132 template <
typename Po
intT,
typename Scalar>
inline void
1135 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid)
1137 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
1140 centroid.setZero (boost::mpl::size<FieldList>::value);
1142 if (indices.empty ())
1146 for (
const auto& index: indices)
1151 centroid /=
static_cast<Scalar
> (indices.size ());
1155 template <
typename Po
intT,
typename Scalar>
inline void
1158 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid)
1163 template <
typename Po
intT>
void
1171 template <
typename Po
intT>
1172 template <
typename Po
intOutT>
void
1175 if (num_points_ != 0)
1179 auto ca = boost::fusion::filter_if<detail::IsAccumulatorCompatible<PointT, PointOutT>> (accumulators_);
1186 template <
typename Po
intInT,
typename Po
intOutT> std::size_t
1188 PointOutT& centroid)
1193 for (
const auto& point: cloud)
1196 for (
const auto& point: cloud)
1205 template <
typename Po
intInT,
typename Po
intOutT> std::size_t
1208 PointOutT& centroid)
1213 for (
const auto &index : indices)
1214 cp.
add (cloud[index]);
1216 for (
const auto &index : indices)
1218 cp.
add (cloud[index]);
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Iterator class for point clouds with or without given indices.
std::size_t getSize() const
Get the total number of points that were added.
std::size_t computeCentroid(const pcl::PointCloud< PointInT > &cloud, PointOutT ¢roid)
Compute the centroid of a set of points and return it as a point.
unsigned int computeMeanAndCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix, Eigen::Matrix< Scalar, 4, 1 > ¢roid)
Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single lo...
Helper functor structure for n-D centroid estimation.
unsigned int computeCentroidAndOBB(const pcl::PointCloud< PointT > &cloud, Eigen::Matrix< Scalar, 3, 1 > ¢roid, Eigen::Matrix< Scalar, 3, 1 > &obb_center, Eigen::Matrix< Scalar, 3, 1 > &obb_dimensions, Eigen::Matrix< Scalar, 3, 3 > &obb_rotational_matrix)
Compute centroid, OBB (Oriented Bounding Box), PCA axes of a given set of points. ...
std::uint32_t width
The point cloud width (if organized as an image-structure).
void add(const PointT &point)
Add a new point to the centroid computation.
void demeanPointCloud(ConstCloudIterator< PointT > &cloud_iterator, const Eigen::Matrix< Scalar, 4, 1 > ¢roid, pcl::PointCloud< PointT > &cloud_out, int npts=0)
Subtract a centroid from a point cloud and return the de-meaned representation.
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.
void computeNDCentroid(const pcl::PointCloud< PointT > &cloud, Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > ¢roid)
General, all purpose nD centroid estimation for a set of points using their indices.
void resize(std::size_t count)
Resizes the container to contain count elements.
std::uint32_t height
The point cloud height (if organized as an image-structure).
IndicesAllocator<> Indices
Type used for indices in PCL.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
pcl::PCLHeader header
The point cloud header.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
A point structure representing Euclidean xyz coordinates, and the RGB color.
unsigned int computeCovarianceMatrixNormalized(const pcl::PointCloud< PointT > &cloud, const Eigen::Matrix< Scalar, 4, 1 > ¢roid, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix)
Compute normalized the 3x3 covariance matrix of a given set of points.
void get(PointOutT &point) const
Retrieve the current centroid.
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > ¢roid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
A generic class that computes the centroid of points fed to it.