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);
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));
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);
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);
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));
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));
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);
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);
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.