47 if (_vertices.
empty())
48 return Eigen::Matrix3d::Identity();
50 Eigen::Matrix<double, 9, 1> cumulants;
52 for (
const auto &vertex : _vertices)
55 cumulants(0) += point(0);
56 cumulants(1) += point(1);
57 cumulants(2) += point(2);
58 cumulants(3) += point(0) * point(0);
59 cumulants(4) += point(0) * point(1);
60 cumulants(5) += point(0) * point(2);
61 cumulants(6) += point(1) * point(1);
62 cumulants(7) += point(1) * point(2);
63 cumulants(8) += point(2) * point(2);
66 Eigen::Matrix3d covariance;
68 cumulants /=
static_cast<double>(_vertices.
size());
70 covariance(0, 0) = cumulants(3) - cumulants(0) * cumulants(0);
71 covariance(1, 1) = cumulants(6) - cumulants(1) * cumulants(1);
72 covariance(2, 2) = cumulants(8) - cumulants(2) * cumulants(2);
73 covariance(0, 1) = cumulants(4) - cumulants(0) * cumulants(1);
74 covariance(1, 0) = covariance(0, 1);
75 covariance(0, 2) = cumulants(5) - cumulants(0) * cumulants(2);
76 covariance(2, 0) = covariance(0, 2);
77 covariance(1, 2) = cumulants(7) - cumulants(1) * cumulants(2);
78 covariance(2, 1) = covariance(1, 2);
93 if (_vertices.
empty())
97 for (
const auto &point : _vertices)
99 mean /=
static_cast<double>(_vertices.
size());
105 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d>
106 eigenSolver(covariance, Eigen::ComputeEigenvectors);
107 Eigen::Matrix3d eigenVectorsPCA = eigenSolver.eigenvectors();
112 eigenVectorsPCA.col(2) =
113 eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1));
117 Eigen::Matrix4d projectionTransform(Eigen::Matrix4d::Identity());
118 projectionTransform.block<3, 3>(0, 0) = eigenVectorsPCA.transpose();
119 projectionTransform.block<3, 1>(0, 3) =
120 -1.0f * (projectionTransform.block<3, 3>(0, 0) * centroid);
126 for (
const auto &point : _vertices)
128 Eigen::Vector4d pt(0, 0, 0, 1);
130 Eigen::Vector4d tfPoint = projectionTransform * pt;
131 minPoint = minPoint.cwiseMin(tfPoint.head<3>());
132 maxPoint = maxPoint.cwiseMax(tfPoint.head<3>());
135 const Eigen::Vector3d meanDiagonal = 0.5f * (maxPoint + minPoint);
140 const Eigen::Quaterniond bboxQuaternion(eigenVectorsPCA);
141 const Eigen::Vector3d bboxTransform =
142 eigenVectorsPCA * meanDiagonal + centroid;
145 maxPoint.x() - minPoint.x(),
146 maxPoint.y() - minPoint.y(),
147 maxPoint.z() - minPoint.z()