Merge f243dd3 into 9622e9e

CMakeLists.txt

@@ -16,7 +16,9 @@ find_package(ignition-cmake3 REQUIRED)
 #============================================================================
 set (c++standard 17)
 set (CMAKE_CXX_STANDARD 17)
-ign_configure_project(VERSION_SUFFIX pre1)
+ign_configure_project(
+  REPLACE_IGNITION_INCLUDE_PATH gz/math
+  VERSION_SUFFIX pre1)
 
 #============================================================================
 # Set project-specific options

eigen3/BUILD.bazel

@@ -11,7 +11,7 @@ package(
 licenses(["notice"])
 
 public_headers = glob([
-    "include/ignition/math/eigen3/*.hh",
+    "include/gz/math/eigen3/*.hh",
 ])
 
 cc_library(

eigen3/include/CMakeLists.txt

@@ -0,0 +1,2 @@
+add_subdirectory(gz)
+install(DIRECTORY ignition DESTINATION ${IGN_INCLUDE_INSTALL_DIR_FULL})

eigen3/include/gz/math/eigen3/Conversions.hh

@@ -0,0 +1,167 @@
+/*
+ * Copyright (C) 2018 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 GZ_MATH_EIGEN3_CONVERSIONS_HH_
+#define GZ_MATH_EIGEN3_CONVERSIONS_HH_
+
+#include <Eigen/Geometry>
+#include <gz/math/AxisAlignedBox.hh>
+#include <gz/math/Matrix3.hh>
+#include <gz/math/Pose3.hh>
+#include <gz/math/Quaternion.hh>
+#include <gz/math/Vector3.hh>
+
+namespace ignition
+{
+  namespace math
+  {
+    namespace eigen3
+    {
+      /// \brief Convert from ignition::math::Vector3d to Eigen::Vector3d.
+      /// \param[in] _v ignition::math::Vector3d to convert
+      /// \return The equivalent Eigen::Vector3d.
+      inline Eigen::Vector3d convert(const ignition::math::Vector3d &_v)
+      {
+        return Eigen::Vector3d(_v[0], _v[1], _v[2]);
+      }
+
+      /// \brief Convert from ignition::math::AxisAlignedBox to
+      /// Eigen::AlignedBox3d.
+      /// \param[in] _b ignition::math::AxisAlignedBox to convert
+      /// \return The equivalent Eigen::AlignedBox3d.
+      inline Eigen::AlignedBox3d convert(
+          const ignition::math::AxisAlignedBox &_b)
+      {
+        return Eigen::AlignedBox3d(convert(_b.Min()), convert(_b.Max()));
+      }
+
+      /// \brief Convert from ignition::math::Matrix3d to Eigen::Matrix3d.
+      /// \param[in] _m ignition::math::Matrix3d to convert.
+      /// \return The equivalent Eigen::Matrix3d.
+      inline Eigen::Matrix3d convert(const ignition::math::Matrix3d &_m)
+      {
+        Eigen::Matrix3d matrix;
+        for (std::size_t i=0; i < 3; ++i)
+        {
+          for (std::size_t j=0; j < 3; ++j)
+          {
+            matrix(i, j) = _m(i, j);
+          }
+        }
+
+        return matrix;
+      }
+
+      /// \brief Convert ignition::math::Quaterniond to Eigen::Quaterniond.
+      /// \param[in] _q ignition::math::Quaterniond to convert.
+      /// \return The equivalent Eigen::Quaterniond.
+      inline Eigen::Quaterniond convert(const ignition::math::Quaterniond &_q)
+      {
+        Eigen::Quaterniond quat;
+        quat.w() = _q.W();
+        quat.x() = _q.X();
+        quat.y() = _q.Y();
+        quat.z() = _q.Z();
+
+        return quat;
+      }
+
+      /// \brief Convert ignition::math::Pose3d to Eigen::Isometry3d.
+      /// \param[in] _pose ignition::math::Pose3d to convert.
+      /// \return The equivalent Eigen::Isometry3d.
+      inline Eigen::Isometry3d convert(const ignition::math::Pose3d &_pose)
+      {
+        Eigen::Isometry3d tf = Eigen::Isometry3d::Identity();
+        tf.translation() = convert(_pose.Pos());
+        tf.linear() = Eigen::Matrix3d(convert(_pose.Rot()));
+
+        return tf;
+      }
+
+      /// \brief Convert Eigen::Vector3d to ignition::math::Vector3d.
+      /// \param[in] _v Eigen::Vector3d to convert.
+      /// \return The equivalent ignition::math::Vector3d.
+      inline ignition::math::Vector3d convert(const Eigen::Vector3d &_v)
+      {
+        ignition::math::Vector3d vec;
+        vec.X() = _v[0];
+        vec.Y() = _v[1];
+        vec.Z() = _v[2];
+
+        return vec;
+      }
+
+      /// \brief Convert Eigen::AlignedBox3d to ignition::math::AxisAlignedBox.
+      /// \param[in] _b Eigen::AlignedBox3d to convert.
+      /// \return The equivalent ignition::math::AxisAlignedBox.
+      inline ignition::math::AxisAlignedBox convert(
+          const Eigen::AlignedBox3d &_b)
+      {
+        ignition::math::AxisAlignedBox box;
+        box.Min() = convert(_b.min());
+        box.Max() = convert(_b.max());
+
+        return box;
+      }
+
+      /// \brief Convert Eigen::Matrix3d to ignition::math::Matrix3d.
+      /// \param[in] _m Eigen::Matrix3d to convert.
+      /// \return The equivalent ignition::math::Matrix3d.
+      inline ignition::math::Matrix3d convert(const Eigen::Matrix3d &_m)
+      {
+        ignition::math::Matrix3d matrix;
+        for (std::size_t i=0; i < 3; ++i)
+        {
+          for (std::size_t j=0; j < 3; ++j)
+          {
+            matrix(i, j) = _m(i, j);
+          }
+        }
+
+        return matrix;
+      }
+
+      /// \brief Convert Eigen::Quaterniond to ignition::math::Quaterniond.
+      /// \param[in] _q Eigen::Quaterniond to convert.
+      /// \return The equivalent ignition::math::Quaterniond.
+      inline ignition::math::Quaterniond convert(const Eigen::Quaterniond &_q)
+      {
+        ignition::math::Quaterniond quat;
+        quat.W() = _q.w();
+        quat.X() = _q.x();
+        quat.Y() = _q.y();
+        quat.Z() = _q.z();
+
+        return quat;
+      }
+
+      /// \brief Convert Eigen::Isometry3d to ignition::math::Pose3d.
+      /// \param[in] _tf Eigen::Isometry3d to convert.
+      /// \return The equivalent ignition::math::Pose3d.
+      inline ignition::math::Pose3d convert(const Eigen::Isometry3d &_tf)
+      {
+        ignition::math::Pose3d pose;
+        pose.Pos() = convert(Eigen::Vector3d(_tf.translation()));
+        pose.Rot() = convert(Eigen::Quaterniond(_tf.linear()));
+
+        return pose;
+      }
+    }
+  }
+}
+
+#endif

eigen3/include/gz/math/eigen3/Util.hh

@@ -0,0 +1,161 @@
+/*
+ * 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 GZ_MATH_EIGEN3_UTIL_HH_
+#define GZ_MATH_EIGEN3_UTIL_HH_
+
+#include <vector>
+
+#include <Eigen/Geometry>
+#include <Eigen/Eigenvalues>
+
+#include <gz/math/AxisAlignedBox.hh>
+#include <gz/math/Matrix3.hh>
+#include <gz/math/OrientedBox.hh>
+#include <gz/math/Pose3.hh>
+#include <gz/math/Quaternion.hh>
+#include <gz/math/Vector3.hh>
+#include <gz/math/eigen3/Conversions.hh>
+
+namespace ignition
+{
+  namespace math
+  {
+    namespace eigen3
+    {
+      /// \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] _vertices a vector of 3d vertices
+      /// \return Covariance matrix
+      inline Eigen::Matrix3d covarianceMatrix(
+        const std::vector<math::Vector3d> &_vertices)
+      {
+        if (_vertices.empty())
+          return Eigen::Matrix3d::Identity();
+
+        Eigen::Matrix<double, 9, 1> cumulants;
+        cumulants.setZero();
+        for (const auto &vertex : _vertices)
+        {
+          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);
+        }
+
+        Eigen::Matrix3d covariance;
+
+        cumulants /= static_cast<double>(_vertices.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 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
+      inline ignition::math::OrientedBoxd verticesToOrientedBox(
+        const std::vector<math::Vector3d> &_vertices)
+      {
+        math::OrientedBoxd box;
+
+        // Return an empty box if there are no vertices
+        if (_vertices.empty())
+          return box;
+
+        math::Vector3d mean;
+        for (const auto &point : _vertices)
+          mean += point;
+        mean /= static_cast<double>(_vertices.size());
+
+        Eigen::Vector3d centroid = math::eigen3::convert(mean);
+        Eigen::Matrix3d covariance = covarianceMatrix(_vertices);
+
+        // Eigen Vectors
+        Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d>
+          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.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 (const auto &point : _vertices)
+        {
+          Eigen::Vector4d pt(0, 0, 0, 1);
+          pt.head<3>() = math::eigen3::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;
+
+        math::Vector3d size(
+            maxPoint.x() - minPoint.x(),
+            maxPoint.y() - minPoint.y(),
+            maxPoint.z() - minPoint.z()
+        );
+        math::Pose3d pose;
+        pose.Rot() = math::eigen3::convert(bboxQuaternion);
+        pose.Pos() = math::eigen3::convert(bboxTransform);
+
+        box.Size(size);
+        box.Pose(pose);
+        return box;
+      }
+    }
+  }
+}
+
+#endif

eigen3/include/ignition/math/eigen3.hh

@@ -0,0 +1,18 @@
+/*
+ * Copyright (C) 2017 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 <gz/math/eigen3.hh>