From a649400fb6f8caa5aab4a9b0bb58bc702ca927df Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Wed, 18 Aug 2021 21:37:58 +0200 Subject: [PATCH 01/11] Add eigen utils to convert mesh 3d vertices to oriented box Signed-off-by: AmrElsersy --- eigen3/include/ignition/math/eigen3/Util.hh | 143 ++++++++++++++++++++ eigen3/src/Util_TEST.cc | 27 ++++ 2 files changed, 170 insertions(+) create mode 100644 eigen3/include/ignition/math/eigen3/Util.hh create mode 100644 eigen3/src/Util_TEST.cc diff --git a/eigen3/include/ignition/math/eigen3/Util.hh b/eigen3/include/ignition/math/eigen3/Util.hh new file mode 100644 index 000000000..f0df0a026 --- /dev/null +++ b/eigen3/include/ignition/math/eigen3/Util.hh @@ -0,0 +1,143 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef IGNITION_MATH_EIGEN3_UTIL_HH_ +#define IGNITION_MATH_EIGEN3_UTIL_HH_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace ignition +{ + namespace math + { + namespace eigen3 + { + inline Eigen::Matrix3d computeCovarianceMatrix( + const std::vector &mesh, + const Eigen::Vector3d ¢roid) + { + Eigen::Matrix3d covariance; + covariance.setZero (); + + // For each 3d vertex in the mesh + for (auto point : mesh) + { + auto pt = convert(point); + pt = pt - centroid; + + covariance(1, 1) += pt.y() * pt.y(); + covariance(1, 2) += pt.y() * pt.z(); + covariance(2, 2) += pt.z() * pt.z(); + + pt *= pt.x (); + covariance(0, 0) += pt.x(); + covariance(0, 1) += pt.y(); + covariance(0, 2) += pt.z(); + } + + covariance(1, 0) = covariance(0, 1); + covariance(2, 0) = covariance(0, 2); + covariance(2, 1) = covariance(1, 2); + + return covariance; + } + + /// \brief Get the oriented 3d bounding box of a mesh points using PCA + inline ignition::math::OrientedBoxd MeshToOrientedBox( + const std::vector &_mesh) + { + math::OrientedBoxd box; + + // Centroid (mean) of the mesh + math::Vector3d mean; + for (auto point : _mesh) + mean += point; + mean /= _mesh.size(); + + Eigen::Vector3d centroid = convert(mean); + + // Covariance + Eigen::Matrix3d covariance = computeCovarianceMatrix(_mesh, centroid); + + // Eigen Vectors + Eigen::SelfAdjointEigenSolver + eigenSolver(covariance, Eigen::ComputeEigenvectors); + Eigen::Matrix3d eigenVectorsPCA = eigenSolver.eigenvectors(); + + // This line is necessary for proper orientation in some cases. + // The numbers come out the same without it, but The signs are + // different and the box doesn't get correctly oriented in some cases. + eigenVectorsPCA.col(2) = + eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1)); + + // Transform the original cloud to the origin where the principal + // components correspond to the axes. + Eigen::Matrix4d projectionTransform(Eigen::Matrix4d::Identity()); + projectionTransform.block<3, 3> (0, 0) = eigenVectorsPCA.transpose(); + projectionTransform.block<3, 1> (0, 3) = + -1.f * (projectionTransform.block<3, 3>(0, 0) * centroid); + + Eigen::Vector3d minPoint(INF_I32, INF_I32, INF_I32); + Eigen::Vector3d maxPoint(-INF_I32, -INF_I32, -INF_I32); + + // Get the minimum and maximum points of the transformed cloud. + for (auto point : _mesh) + { + Eigen::Vector4d pt(0, 0, 0, 1); + pt.head<3>() = convert(point); + Eigen::Vector4d tfPoint = projectionTransform * pt; + minPoint = minPoint.cwiseMin(tfPoint.head<3>()); + maxPoint = maxPoint.cwiseMax(tfPoint.head<3>()); + } + + const Eigen::Vector3d meanDiagonal = 0.5f*(maxPoint + minPoint); + + // quaternion is calculated using the eigenvectors (which determines + // how the final box gets rotated), and the transform to put the box + // in correct location is calculated + const Eigen::Quaterniond bboxQuaternion(eigenVectorsPCA); + const Eigen::Vector3d bboxTransform = + eigenVectorsPCA * meanDiagonal + centroid; + + // xyz xzy ... yxz yzx ... zyx zxy + math::Vector3d size( + maxPoint.x() - minPoint.x(), + maxPoint.y() - minPoint.y(), + maxPoint.z() - minPoint.z() + ); + math::Pose3d pose; + pose.Rot() = convert(bboxQuaternion); + pose.Pos() = convert(bboxTransform); + + box.Size(size); + box.Pose(pose); + return box; + } + } + } +} + +#endif diff --git a/eigen3/src/Util_TEST.cc b/eigen3/src/Util_TEST.cc new file mode 100644 index 000000000..1498c5457 --- /dev/null +++ b/eigen3/src/Util_TEST.cc @@ -0,0 +1,27 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + + +#include + +#include + +///////////////////////////////////////////////// +/// Check Vector3 conversions +TEST(EigenUtil, ray2) +{ +} From e548258b1581fcf28d560bd988f35657613e87b1 Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Thu, 19 Aug 2021 01:57:23 +0200 Subject: [PATCH 02/11] replace covariance implementation & style Signed-off-by: AmrElsersy --- eigen3/include/ignition/math/eigen3/Util.hh | 73 ++++++++++++--------- eigen3/src/Util_TEST.cc | 4 +- 2 files changed, 42 insertions(+), 35 deletions(-) diff --git a/eigen3/include/ignition/math/eigen3/Util.hh b/eigen3/include/ignition/math/eigen3/Util.hh index f0df0a026..3fe72b654 100644 --- a/eigen3/include/ignition/math/eigen3/Util.hh +++ b/eigen3/include/ignition/math/eigen3/Util.hh @@ -19,15 +19,17 @@ #define IGNITION_MATH_EIGEN3_UTIL_HH_ #include + #include #include + #include #include +#include #include #include #include #include -#include namespace ignition { @@ -35,52 +37,60 @@ namespace ignition { namespace eigen3 { - inline Eigen::Matrix3d computeCovarianceMatrix( - const std::vector &mesh, - const Eigen::Vector3d ¢roid) + /// \brief Get covariance matrix from a set of 3d vertices + /// https://github.com/isl-org/Open3D/blob/76c2baf9debd460900f056a9b51e9a80de9c0e64/cpp/open3d/utility/Eigen.cpp#L305 + /// \param[in] mesh a mesh of 3d vertices + /// \param[in] centroid Mean or centeroid of the 3d vertices + /// \return Covariance matrix + inline Eigen::Matrix3d covarianceMatrix( + const std::vector &_mesh) { Eigen::Matrix3d covariance; - covariance.setZero (); - - // For each 3d vertex in the mesh - for (auto point : mesh) + Eigen::Matrix cumulants; + cumulants.setZero(); + for (const auto &vertex : _mesh) { - auto pt = convert(point); - pt = pt - centroid; - - covariance(1, 1) += pt.y() * pt.y(); - covariance(1, 2) += pt.y() * pt.z(); - covariance(2, 2) += pt.z() * pt.z(); - - pt *= pt.x (); - covariance(0, 0) += pt.x(); - covariance(0, 1) += pt.y(); - covariance(0, 2) += pt.z(); + const Eigen::Vector3d &point = math::eigen3::convert(vertex); + cumulants(0) += point(0); + cumulants(1) += point(1); + cumulants(2) += point(2); + cumulants(3) += point(0) * point(0); + cumulants(4) += point(0) * point(1); + cumulants(5) += point(0) * point(2); + cumulants(6) += point(1) * point(1); + cumulants(7) += point(1) * point(2); + cumulants(8) += point(2) * point(2); } + cumulants /= static_cast(_mesh.size()); + covariance(0, 0) = cumulants(3) - cumulants(0) * cumulants(0); + covariance(1, 1) = cumulants(6) - cumulants(1) * cumulants(1); + covariance(2, 2) = cumulants(8) - cumulants(2) * cumulants(2); + covariance(0, 1) = cumulants(4) - cumulants(0) * cumulants(1); covariance(1, 0) = covariance(0, 1); + covariance(0, 2) = cumulants(5) - cumulants(0) * cumulants(2); covariance(2, 0) = covariance(0, 2); + covariance(1, 2) = cumulants(7) - cumulants(1) * cumulants(2); covariance(2, 1) = covariance(1, 2); - return covariance; } /// \brief Get the oriented 3d bounding box of a mesh points using PCA - inline ignition::math::OrientedBoxd MeshToOrientedBox( + /// http://codextechnicanum.blogspot.com/2015/04/find-minimum-oriented-bounding-box-of.html + /// \param[in] mesh a mesh of 3d vertices + /// \return Oriented 3D box + inline ignition::math::OrientedBoxd meshToOrientedBox( const std::vector &_mesh) { math::OrientedBoxd box; - // Centroid (mean) of the mesh math::Vector3d mean; for (auto point : _mesh) mean += point; mean /= _mesh.size(); - Eigen::Vector3d centroid = convert(mean); - - // Covariance - Eigen::Matrix3d covariance = computeCovarianceMatrix(_mesh, centroid); + Eigen::Vector3d centroid = math::eigen3::convert(mean); + Eigen::Matrix3d covariance = covarianceMatrix(_mesh); // Eigen Vectors Eigen::SelfAdjointEigenSolver @@ -96,15 +106,15 @@ namespace ignition // Transform the original cloud to the origin where the principal // components correspond to the axes. Eigen::Matrix4d projectionTransform(Eigen::Matrix4d::Identity()); - projectionTransform.block<3, 3> (0, 0) = eigenVectorsPCA.transpose(); - projectionTransform.block<3, 1> (0, 3) = - -1.f * (projectionTransform.block<3, 3>(0, 0) * centroid); + projectionTransform.block<3, 3>(0, 0) = eigenVectorsPCA.transpose(); + projectionTransform.block<3, 1>(0, 3) = + -1.0f * (projectionTransform.block<3, 3>(0, 0) * centroid); Eigen::Vector3d minPoint(INF_I32, INF_I32, INF_I32); Eigen::Vector3d maxPoint(-INF_I32, -INF_I32, -INF_I32); // Get the minimum and maximum points of the transformed cloud. - for (auto point : _mesh) + for (const auto &point : _mesh) { Eigen::Vector4d pt(0, 0, 0, 1); pt.head<3>() = convert(point); @@ -113,7 +123,7 @@ namespace ignition maxPoint = maxPoint.cwiseMax(tfPoint.head<3>()); } - const Eigen::Vector3d meanDiagonal = 0.5f*(maxPoint + minPoint); + const Eigen::Vector3d meanDiagonal = 0.5f * (maxPoint + minPoint); // quaternion is calculated using the eigenvectors (which determines // how the final box gets rotated), and the transform to put the box @@ -122,7 +132,6 @@ namespace ignition const Eigen::Vector3d bboxTransform = eigenVectorsPCA * meanDiagonal + centroid; - // xyz xzy ... yxz yzx ... zyx zxy math::Vector3d size( maxPoint.x() - minPoint.x(), maxPoint.y() - minPoint.y(), diff --git a/eigen3/src/Util_TEST.cc b/eigen3/src/Util_TEST.cc index 1498c5457..25e76fe55 100644 --- a/eigen3/src/Util_TEST.cc +++ b/eigen3/src/Util_TEST.cc @@ -15,13 +15,11 @@ * */ - #include #include ///////////////////////////////////////////////// -/// Check Vector3 conversions -TEST(EigenUtil, ray2) +TEST(EigenUtil, MeshToOrientedBox) { } From 616998af6d8e27d181e4c2b151e974d8a392eab7 Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Thu, 19 Aug 2021 02:34:22 +0200 Subject: [PATCH 03/11] Add Test Signed-off-by: AmrElsersy --- eigen3/src/Util_TEST.cc | 39 ++++++++++++++++++++++++++++++++++++++- 1 file changed, 38 insertions(+), 1 deletion(-) diff --git a/eigen3/src/Util_TEST.cc b/eigen3/src/Util_TEST.cc index 25e76fe55..dcd64020d 100644 --- a/eigen3/src/Util_TEST.cc +++ b/eigen3/src/Util_TEST.cc @@ -19,7 +19,44 @@ #include +using namespace ignition; + ///////////////////////////////////////////////// -TEST(EigenUtil, MeshToOrientedBox) +/// \brief Test the oriented box converted from a set of vertices +TEST(EigenUtil, meshToOrientedBox) { + std::vector mesh; + + mesh.push_back(math::Vector3d(1, 0, 0.5)); + mesh.push_back(math::Vector3d(2, 0.1, 0.4)); + mesh.push_back(math::Vector3d(2, 1, 3)); + mesh.push_back(math::Vector3d(1.6, 0.3, 0.1)); + mesh.push_back(math::Vector3d(1.5, 0.5, 1)); + mesh.push_back(math::Vector3d(1.4, 1, 3)); + mesh.push_back(math::Vector3d(1, 0.4, 0.7)); + mesh.push_back(math::Vector3d(0.9, 1.3, 0)); + mesh.push_back(math::Vector3d(0.6, 4, 2)); + mesh.push_back(math::Vector3d(0, 3, 3)); + mesh.push_back(math::Vector3d(-1, -2, 4)); + mesh.push_back(math::Vector3d(-2, -2, 0.6)); + + math::OrientedBoxd box = math::eigen3::meshToOrientedBox(mesh); + + auto position = box.Pose().Pos(); + auto rotation = box.Pose().Rot(); + auto size = box.Size(); + + double error = 0.1; + + EXPECT_NEAR(size.X(), 3.09, error); + EXPECT_NEAR(size.Y(), 4.39, error); + EXPECT_NEAR(size.Z(), 6.63, error); + + EXPECT_NEAR(position.X(), 0.38, error); + EXPECT_NEAR(position.Y(), 0.47, error); + EXPECT_NEAR(position.Z(), 1.96, error); + + EXPECT_NEAR(rotation.Roll(), -1.66, error); + EXPECT_NEAR(rotation.Pitch(), 0.4, error); + EXPECT_NEAR(rotation.Yaw(), 2.7, error); } From 092666292b4c3381130c4371d106b0597b14f4f6 Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Thu, 19 Aug 2021 12:51:08 +0200 Subject: [PATCH 04/11] style Signed-off-by: AmrElsersy --- eigen3/include/ignition/math/eigen3/Util.hh | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/eigen3/include/ignition/math/eigen3/Util.hh b/eigen3/include/ignition/math/eigen3/Util.hh index 3fe72b654..da30444da 100644 --- a/eigen3/include/ignition/math/eigen3/Util.hh +++ b/eigen3/include/ignition/math/eigen3/Util.hh @@ -39,13 +39,11 @@ namespace ignition { /// \brief Get covariance matrix from a set of 3d vertices /// https://github.com/isl-org/Open3D/blob/76c2baf9debd460900f056a9b51e9a80de9c0e64/cpp/open3d/utility/Eigen.cpp#L305 - /// \param[in] mesh a mesh of 3d vertices - /// \param[in] centroid Mean or centeroid of the 3d vertices + /// \param[in] _mesh a mesh of 3d vertices /// \return Covariance matrix inline Eigen::Matrix3d covarianceMatrix( const std::vector &_mesh) { - Eigen::Matrix3d covariance; Eigen::Matrix cumulants; cumulants.setZero(); for (const auto &vertex : _mesh) @@ -63,6 +61,8 @@ namespace ignition } cumulants /= static_cast(_mesh.size()); + + Eigen::Matrix3d covariance; covariance(0, 0) = cumulants(3) - cumulants(0) * cumulants(0); covariance(1, 1) = cumulants(6) - cumulants(1) * cumulants(1); covariance(2, 2) = cumulants(8) - cumulants(2) * cumulants(2); @@ -77,17 +77,15 @@ namespace ignition /// \brief Get the oriented 3d bounding box of a mesh points using PCA /// http://codextechnicanum.blogspot.com/2015/04/find-minimum-oriented-bounding-box-of.html - /// \param[in] mesh a mesh of 3d vertices + /// \param[in] _mesh a mesh of 3d vertices /// \return Oriented 3D box inline ignition::math::OrientedBoxd meshToOrientedBox( const std::vector &_mesh) { - math::OrientedBoxd box; - math::Vector3d mean; - for (auto point : _mesh) + for (const auto &point : _mesh) mean += point; - mean /= _mesh.size(); + mean /= static_cast(_mesh.size()); Eigen::Vector3d centroid = math::eigen3::convert(mean); Eigen::Matrix3d covariance = covarianceMatrix(_mesh); @@ -138,8 +136,10 @@ namespace ignition maxPoint.z() - minPoint.z() ); math::Pose3d pose; - pose.Rot() = convert(bboxQuaternion); - pose.Pos() = convert(bboxTransform); + pose.Rot() = math::eigen3::convert(bboxQuaternion); + pose.Pos() = math::eigen3::convert(bboxTransform); + + math::OrientedBoxd box; box.Size(size); box.Pose(pose); From ff57eb9c1879eae1fc1cb9751e1cbd50dae636b2 Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Thu, 19 Aug 2021 19:44:38 +0200 Subject: [PATCH 05/11] naming style Signed-off-by: AmrElsersy --- eigen3/include/ignition/math/eigen3/Util.hh | 32 ++++++++++++--------- eigen3/src/Util_TEST.cc | 4 +-- 2 files changed, 20 insertions(+), 16 deletions(-) diff --git a/eigen3/include/ignition/math/eigen3/Util.hh b/eigen3/include/ignition/math/eigen3/Util.hh index da30444da..4ff9d47df 100644 --- a/eigen3/include/ignition/math/eigen3/Util.hh +++ b/eigen3/include/ignition/math/eigen3/Util.hh @@ -39,14 +39,14 @@ namespace ignition { /// \brief Get covariance matrix from a set of 3d vertices /// https://github.com/isl-org/Open3D/blob/76c2baf9debd460900f056a9b51e9a80de9c0e64/cpp/open3d/utility/Eigen.cpp#L305 - /// \param[in] _mesh a mesh of 3d vertices + /// \param[in] _vertices a vector of 3d vertices /// \return Covariance matrix inline Eigen::Matrix3d covarianceMatrix( - const std::vector &_mesh) + const std::vector &_vertices) { Eigen::Matrix cumulants; cumulants.setZero(); - for (const auto &vertex : _mesh) + for (const auto &vertex : _vertices) { const Eigen::Vector3d &point = math::eigen3::convert(vertex); cumulants(0) += point(0); @@ -60,7 +60,7 @@ namespace ignition cumulants(8) += point(2) * point(2); } - cumulants /= static_cast(_mesh.size()); + cumulants /= static_cast(_vertices.size()); Eigen::Matrix3d covariance; covariance(0, 0) = cumulants(3) - cumulants(0) * cumulants(0); @@ -75,20 +75,26 @@ namespace ignition return covariance; } - /// \brief Get the oriented 3d bounding box of a mesh points using PCA + /// \brief Get the oriented 3d bounding box of a 3d vertices using PCA /// http://codextechnicanum.blogspot.com/2015/04/find-minimum-oriented-bounding-box-of.html - /// \param[in] _mesh a mesh of 3d vertices + /// \param[in] _vertices a vector of 3d vertices /// \return Oriented 3D box - inline ignition::math::OrientedBoxd meshToOrientedBox( - const std::vector &_mesh) + inline ignition::math::OrientedBoxd verticesToOrientedBox( + const std::vector &_vertices) { + math::OrientedBoxd box; + + // Return an empty box if there are no vertices + if (_vertices.size() == 0) + return box; + math::Vector3d mean; - for (const auto &point : _mesh) + for (const auto &point : _vertices) mean += point; - mean /= static_cast(_mesh.size()); + mean /= static_cast(_vertices.size()); Eigen::Vector3d centroid = math::eigen3::convert(mean); - Eigen::Matrix3d covariance = covarianceMatrix(_mesh); + Eigen::Matrix3d covariance = covarianceMatrix(_vertices); // Eigen Vectors Eigen::SelfAdjointEigenSolver @@ -112,7 +118,7 @@ namespace ignition Eigen::Vector3d maxPoint(-INF_I32, -INF_I32, -INF_I32); // Get the minimum and maximum points of the transformed cloud. - for (const auto &point : _mesh) + for (const auto &point : _vertices) { Eigen::Vector4d pt(0, 0, 0, 1); pt.head<3>() = convert(point); @@ -139,8 +145,6 @@ namespace ignition pose.Rot() = math::eigen3::convert(bboxQuaternion); pose.Pos() = math::eigen3::convert(bboxTransform); - math::OrientedBoxd box; - box.Size(size); box.Pose(pose); return box; diff --git a/eigen3/src/Util_TEST.cc b/eigen3/src/Util_TEST.cc index dcd64020d..013e00d1b 100644 --- a/eigen3/src/Util_TEST.cc +++ b/eigen3/src/Util_TEST.cc @@ -23,7 +23,7 @@ using namespace ignition; ///////////////////////////////////////////////// /// \brief Test the oriented box converted from a set of vertices -TEST(EigenUtil, meshToOrientedBox) +TEST(EigenUtil, verticesToOrientedBox) { std::vector mesh; @@ -40,7 +40,7 @@ TEST(EigenUtil, meshToOrientedBox) mesh.push_back(math::Vector3d(-1, -2, 4)); mesh.push_back(math::Vector3d(-2, -2, 0.6)); - math::OrientedBoxd box = math::eigen3::meshToOrientedBox(mesh); + math::OrientedBoxd box = math::eigen3::verticesToOrientedBox(mesh); auto position = box.Pose().Pos(); auto rotation = box.Pose().Rot(); From e9b854d83fe093af4a2d95d8c3e6ab650f1fe376 Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Thu, 19 Aug 2021 19:46:08 +0200 Subject: [PATCH 06/11] devide by zero checking Signed-off-by: AmrElsersy --- eigen3/include/ignition/math/eigen3/Util.hh | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/eigen3/include/ignition/math/eigen3/Util.hh b/eigen3/include/ignition/math/eigen3/Util.hh index 4ff9d47df..909fe2661 100644 --- a/eigen3/include/ignition/math/eigen3/Util.hh +++ b/eigen3/include/ignition/math/eigen3/Util.hh @@ -60,9 +60,12 @@ namespace ignition cumulants(8) += point(2) * point(2); } + Eigen::Matrix3d covariance; + if (_vertices.size() == 0) + return covariance; + cumulants /= static_cast(_vertices.size()); - Eigen::Matrix3d covariance; covariance(0, 0) = cumulants(3) - cumulants(0) * cumulants(0); covariance(1, 1) = cumulants(6) - cumulants(1) * cumulants(1); covariance(2, 2) = cumulants(8) - cumulants(2) * cumulants(2); From 45a4766cbeaa25aec1fd506291f66483817a4590 Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Fri, 27 Aug 2021 03:15:16 +0200 Subject: [PATCH 07/11] return zero matrix Signed-off-by: AmrElsersy --- eigen3/include/ignition/math/eigen3/Util.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/eigen3/include/ignition/math/eigen3/Util.hh b/eigen3/include/ignition/math/eigen3/Util.hh index 909fe2661..783b18b05 100644 --- a/eigen3/include/ignition/math/eigen3/Util.hh +++ b/eigen3/include/ignition/math/eigen3/Util.hh @@ -62,7 +62,7 @@ namespace ignition Eigen::Matrix3d covariance; if (_vertices.size() == 0) - return covariance; + return Eigen::Matrix3d::Zero(); cumulants /= static_cast(_vertices.size()); From d0932a28df0efc55b14a3dc742949b5d484a38da Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Fri, 27 Aug 2021 23:48:58 +0200 Subject: [PATCH 08/11] style Signed-off-by: AmrElsersy --- eigen3/include/ignition/math/eigen3/Util.hh | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/eigen3/include/ignition/math/eigen3/Util.hh b/eigen3/include/ignition/math/eigen3/Util.hh index 783b18b05..2dad81dbc 100644 --- a/eigen3/include/ignition/math/eigen3/Util.hh +++ b/eigen3/include/ignition/math/eigen3/Util.hh @@ -44,6 +44,9 @@ namespace ignition inline Eigen::Matrix3d covarianceMatrix( const std::vector &_vertices) { + if (_vertices.empty()) + return Eigen::Matrix3d::Identity(); + Eigen::Matrix cumulants; cumulants.setZero(); for (const auto &vertex : _vertices) @@ -61,8 +64,6 @@ namespace ignition } Eigen::Matrix3d covariance; - if (_vertices.size() == 0) - return Eigen::Matrix3d::Zero(); cumulants /= static_cast(_vertices.size()); @@ -78,7 +79,8 @@ namespace ignition return covariance; } - /// \brief Get the oriented 3d bounding box of a 3d vertices using PCA + /// \brief Get the oriented 3d bounding box of a set of 3d + /// vertices using PCA /// http://codextechnicanum.blogspot.com/2015/04/find-minimum-oriented-bounding-box-of.html /// \param[in] _vertices a vector of 3d vertices /// \return Oriented 3D box @@ -88,7 +90,7 @@ namespace ignition math::OrientedBoxd box; // Return an empty box if there are no vertices - if (_vertices.size() == 0) + if (_vertices.empty()) return box; math::Vector3d mean; @@ -105,7 +107,7 @@ namespace ignition Eigen::Matrix3d eigenVectorsPCA = eigenSolver.eigenvectors(); // This line is necessary for proper orientation in some cases. - // The numbers come out the same without it, but The signs are + // The numbers come out the same without it, but the signs are // different and the box doesn't get correctly oriented in some cases. eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1)); From 12824bd9ae2d2c7a2237fcbdc5dd43c2de22dc56 Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Sat, 28 Aug 2021 02:24:53 +0200 Subject: [PATCH 09/11] Empty Testing + Simple Box Testing Signed-off-by: AmrElsersy --- eigen3/src/Util_TEST.cc | 123 ++++++++++++++++++++++++++++++++++------ 1 file changed, 107 insertions(+), 16 deletions(-) diff --git a/eigen3/src/Util_TEST.cc b/eigen3/src/Util_TEST.cc index 013e00d1b..89d626183 100644 --- a/eigen3/src/Util_TEST.cc +++ b/eigen3/src/Util_TEST.cc @@ -25,22 +25,23 @@ using namespace ignition; /// \brief Test the oriented box converted from a set of vertices TEST(EigenUtil, verticesToOrientedBox) { - std::vector mesh; - - mesh.push_back(math::Vector3d(1, 0, 0.5)); - mesh.push_back(math::Vector3d(2, 0.1, 0.4)); - mesh.push_back(math::Vector3d(2, 1, 3)); - mesh.push_back(math::Vector3d(1.6, 0.3, 0.1)); - mesh.push_back(math::Vector3d(1.5, 0.5, 1)); - mesh.push_back(math::Vector3d(1.4, 1, 3)); - mesh.push_back(math::Vector3d(1, 0.4, 0.7)); - mesh.push_back(math::Vector3d(0.9, 1.3, 0)); - mesh.push_back(math::Vector3d(0.6, 4, 2)); - mesh.push_back(math::Vector3d(0, 3, 3)); - mesh.push_back(math::Vector3d(-1, -2, 4)); - mesh.push_back(math::Vector3d(-2, -2, 0.6)); - - math::OrientedBoxd box = math::eigen3::verticesToOrientedBox(mesh); + std::vector vertices; + + vertices.push_back(math::Vector3d(1, 0, 0.5)); + vertices.push_back(math::Vector3d(2, 0.1, 0.4)); + vertices.push_back(math::Vector3d(2, 1, 3)); + vertices.push_back(math::Vector3d(1.6, 0.3, 0.1)); + vertices.push_back(math::Vector3d(1.5, 0.5, 1)); + vertices.push_back(math::Vector3d(1.4, 1, 3)); + vertices.push_back(math::Vector3d(1, 0.4, 0.7)); + vertices.push_back(math::Vector3d(0.9, 1.3, 0)); + vertices.push_back(math::Vector3d(0.6, 4, 2)); + vertices.push_back(math::Vector3d(0, 3, 3)); + vertices.push_back(math::Vector3d(-1, -2, 4)); + vertices.push_back(math::Vector3d(-2, -2, 0.6)); + + math::OrientedBoxd box = math::eigen3::verticesToOrientedBox( + vertices); auto position = box.Pose().Pos(); auto rotation = box.Pose().Rot(); @@ -60,3 +61,93 @@ TEST(EigenUtil, verticesToOrientedBox) EXPECT_NEAR(rotation.Pitch(), 0.4, error); EXPECT_NEAR(rotation.Yaw(), 2.7, error); } + +///////////////////////////////////////////////// +TEST(EigenUtil, emptyVertices) +{ + std::vector emptyVertices; + + math::OrientedBoxd box = math::eigen3::verticesToOrientedBox( + emptyVertices); + + auto position = box.Pose().Pos(); + auto rotation = box.Pose().Rot(); + auto size = box.Size(); + + EXPECT_DOUBLE_EQ(size.X(), 0); + EXPECT_DOUBLE_EQ(size.Y(), 0); + EXPECT_DOUBLE_EQ(size.Z(), 0); + + EXPECT_DOUBLE_EQ(position.X(), 0); + EXPECT_DOUBLE_EQ(position.Y(), 0); + EXPECT_DOUBLE_EQ(position.Z(), 0); + + EXPECT_DOUBLE_EQ(rotation.Roll(), 0); + EXPECT_DOUBLE_EQ(rotation.Pitch(),0); + EXPECT_DOUBLE_EQ(rotation.Yaw(), 0); +} + +///////////////////////////////////////////////// +TEST(EigenUtil, simpleBox) +{ + std::vector vertices; + + vertices.push_back(math::Vector3d(-1,-1,-1)); + vertices.push_back(math::Vector3d(-1,1,-1)); + vertices.push_back(math::Vector3d(1,-1,-1)); + vertices.push_back(math::Vector3d(1,1,-1)); + vertices.push_back(math::Vector3d(-1,-1,1)); + vertices.push_back(math::Vector3d(-1,1,1)); + vertices.push_back(math::Vector3d(1,-1,1)); + vertices.push_back(math::Vector3d(1,1,1)); + + math::OrientedBoxd box = math::eigen3::verticesToOrientedBox( + vertices); + + auto position = box.Pose().Pos(); + auto rotation = box.Pose().Rot(); + auto size = box.Size(); + + double error = 0.1; + + EXPECT_NEAR(size.X(), 2, error); + EXPECT_NEAR(size.Y(), 2, error); + EXPECT_NEAR(size.Z(), 2, error); + + EXPECT_NEAR(position.X(), 0, error); + EXPECT_NEAR(position.Y(), 0, error); + EXPECT_NEAR(position.Z(), 0, error); + + EXPECT_NEAR(rotation.Roll(), 0, error); + EXPECT_NEAR(rotation.Pitch(), 0, error); + EXPECT_NEAR(rotation.Yaw(), 0, error); +} + +///////////////////////////////////////////////// +TEST(EigenUtil, covarianceTest) +{ + std::vector vertices; + + vertices.push_back(math::Vector3d(1, 0, 0.5)); + vertices.push_back(math::Vector3d(2, 0.1, 0.4)); + vertices.push_back(math::Vector3d(2, 1, 3)); + vertices.push_back(math::Vector3d(1.6, 0.3, 0.1)); + vertices.push_back(math::Vector3d(1.5, 0.5, 1)); + vertices.push_back(math::Vector3d(1.4, 1, 3)); + vertices.push_back(math::Vector3d(1, 0.4, 0.7)); + + Eigen::Matrix3d covariance = math::eigen3::covarianceMatrix( + vertices); + + double error = 0.1; + + EXPECT_NEAR(covariance(0), 0.145714, error); + EXPECT_NEAR(covariance(1), 0.04, error); + EXPECT_NEAR(covariance(2), 0.115714, error); + EXPECT_NEAR(covariance(3), 0.04, error); + EXPECT_NEAR(covariance(4), 0.136327, error); + EXPECT_NEAR(covariance(5), 0.392653, error); + EXPECT_NEAR(covariance(6), 0.115714, error); + EXPECT_NEAR(covariance(7), 0.392653, error); + EXPECT_NEAR(covariance(8), 1.29959, error); +} From e9fd22a269ba89ce752a6e5a3c03776b0032c6ed Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Tue, 31 Aug 2021 18:19:02 +0200 Subject: [PATCH 10/11] add empty covariance test Signed-off-by: AmrElsersy --- eigen3/include/ignition/math/eigen3/Util.hh | 2 +- eigen3/src/Util_TEST.cc | 23 +++++++++++++++++++-- 2 files changed, 22 insertions(+), 3 deletions(-) diff --git a/eigen3/include/ignition/math/eigen3/Util.hh b/eigen3/include/ignition/math/eigen3/Util.hh index 2dad81dbc..25f740bd8 100644 --- a/eigen3/include/ignition/math/eigen3/Util.hh +++ b/eigen3/include/ignition/math/eigen3/Util.hh @@ -126,7 +126,7 @@ namespace ignition for (const auto &point : _vertices) { Eigen::Vector4d pt(0, 0, 0, 1); - pt.head<3>() = convert(point); + pt.head<3>() = math::eigen3::convert(point); Eigen::Vector4d tfPoint = projectionTransform * pt; minPoint = minPoint.cwiseMin(tfPoint.head<3>()); maxPoint = maxPoint.cwiseMax(tfPoint.head<3>()); diff --git a/eigen3/src/Util_TEST.cc b/eigen3/src/Util_TEST.cc index 89d626183..c28b6a816 100644 --- a/eigen3/src/Util_TEST.cc +++ b/eigen3/src/Util_TEST.cc @@ -83,8 +83,8 @@ TEST(EigenUtil, emptyVertices) EXPECT_DOUBLE_EQ(position.Z(), 0); EXPECT_DOUBLE_EQ(rotation.Roll(), 0); - EXPECT_DOUBLE_EQ(rotation.Pitch(),0); - EXPECT_DOUBLE_EQ(rotation.Yaw(), 0); + EXPECT_DOUBLE_EQ(rotation.Pitch(), 0); + EXPECT_DOUBLE_EQ(rotation.Yaw(), 0); } ///////////////////////////////////////////////// @@ -151,3 +151,22 @@ TEST(EigenUtil, covarianceTest) EXPECT_NEAR(covariance(7), 0.392653, error); EXPECT_NEAR(covariance(8), 1.29959, error); } + +///////////////////////////////////////////////// +TEST(EigenUtil, covarianceEmptyTest) +{ + std::vector vertices; + + Eigen::Matrix3d covariance = math::eigen3::covarianceMatrix( + vertices); + + EXPECT_DOUBLE_EQ(covariance(0), 1); + EXPECT_DOUBLE_EQ(covariance(1), 0); + EXPECT_DOUBLE_EQ(covariance(2), 0); + EXPECT_DOUBLE_EQ(covariance(3), 0); + EXPECT_DOUBLE_EQ(covariance(4), 1); + EXPECT_DOUBLE_EQ(covariance(5), 0); + EXPECT_DOUBLE_EQ(covariance(6), 0); + EXPECT_DOUBLE_EQ(covariance(7), 0); + EXPECT_DOUBLE_EQ(covariance(8), 1); +} From ee7f15669b05fc4f04bc0e10a307f5d35d115b6b Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 3 Sep 2021 17:11:24 -0700 Subject: [PATCH 11/11] fix codecheck Signed-off-by: Steve Peters --- eigen3/src/Util_TEST.cc | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/eigen3/src/Util_TEST.cc b/eigen3/src/Util_TEST.cc index c28b6a816..1e5220462 100644 --- a/eigen3/src/Util_TEST.cc +++ b/eigen3/src/Util_TEST.cc @@ -92,14 +92,14 @@ TEST(EigenUtil, simpleBox) { std::vector vertices; - vertices.push_back(math::Vector3d(-1,-1,-1)); - vertices.push_back(math::Vector3d(-1,1,-1)); - vertices.push_back(math::Vector3d(1,-1,-1)); - vertices.push_back(math::Vector3d(1,1,-1)); - vertices.push_back(math::Vector3d(-1,-1,1)); - vertices.push_back(math::Vector3d(-1,1,1)); - vertices.push_back(math::Vector3d(1,-1,1)); - vertices.push_back(math::Vector3d(1,1,1)); + vertices.push_back(math::Vector3d(-1, -1, -1)); + vertices.push_back(math::Vector3d(-1, 1, -1)); + vertices.push_back(math::Vector3d(1, -1, -1)); + vertices.push_back(math::Vector3d(1, 1, -1)); + vertices.push_back(math::Vector3d(-1, -1, 1)); + vertices.push_back(math::Vector3d(-1, 1, 1)); + vertices.push_back(math::Vector3d(1, -1, 1)); + vertices.push_back(math::Vector3d(1, 1, 1)); math::OrientedBoxd box = math::eigen3::verticesToOrientedBox( vertices);