Adding minimum volume oriented bounding ellipsoid matching functionality

cpp/open3d/geometry/BoundingVolume.cpp

@@ -15,11 +15,115 @@
 #include "open3d/geometry/Qhull.h"
 #include "open3d/geometry/TriangleMesh.h"
 #include "open3d/t/geometry/kernel/MinimumOBB.h"
+#include "open3d/t/geometry/kernel/MinimumOBE.h"
 #include "open3d/utility/Logging.h"
 
 namespace open3d {
 namespace geometry {
 
+OrientedBoundingEllipsoid& OrientedBoundingEllipsoid::Clear() {
+    center_.setZero();
+    radii_.setZero();
+    R_ = Eigen::Matrix3d::Identity();
+    color_.setOnes();
+    return *this;
+}
+
+bool OrientedBoundingEllipsoid::IsEmpty() const { return Volume() <= 0; }
+
+Eigen::Vector3d OrientedBoundingEllipsoid::GetMinBound() const {
+    auto points = GetEllipsoidPoints();
+    return ComputeMinBound(points);
+}
+
+Eigen::Vector3d OrientedBoundingEllipsoid::GetMaxBound() const {
+    auto points = GetEllipsoidPoints();
+    return ComputeMaxBound(points);
+}
+
+Eigen::Vector3d OrientedBoundingEllipsoid::GetCenter() const { return center_; }
+
+AxisAlignedBoundingBox OrientedBoundingEllipsoid::GetAxisAlignedBoundingBox()
+        const {
+    return AxisAlignedBoundingBox::CreateFromPoints(GetEllipsoidPoints());
+}
+
+OrientedBoundingBox OrientedBoundingEllipsoid::GetOrientedBoundingBox(
+        bool) const {
+    return OrientedBoundingBox::CreateFromPoints(GetEllipsoidPoints());
+}
+
+OrientedBoundingBox OrientedBoundingEllipsoid::GetMinimalOrientedBoundingBox(
+        bool robust) const {
+    return OrientedBoundingBox::CreateFromPoints(GetEllipsoidPoints());
+}
+
+OrientedBoundingEllipsoid
+OrientedBoundingEllipsoid::GetOrientedBoundingEllipsoid(bool) const {
+    return *this;
+}
+
+OrientedBoundingEllipsoid& OrientedBoundingEllipsoid::Transform(
+        const Eigen::Matrix4d& transformation) {
+    utility::LogError(
+            "A general transform of an OrientedBoundingEllipsoid is not "
+            "implemented. "
+            "Call Translate, Scale, and Rotate.");
+    return *this;
+}
+
+OrientedBoundingEllipsoid& OrientedBoundingEllipsoid::Translate(
+        const Eigen::Vector3d& translation, bool relative) {
+    if (relative) {
+        center_ += translation;
+    } else {
+        center_ = translation;
+    }
+    return *this;
+}
+
+OrientedBoundingEllipsoid& OrientedBoundingEllipsoid::Scale(
+        const double scale, const Eigen::Vector3d& center) {
+    radii_ *= scale;
+    center_ = scale * (center_ - center) + center;
+    return *this;
+}
+
+OrientedBoundingEllipsoid& OrientedBoundingEllipsoid::Rotate(
+        const Eigen::Matrix3d& R, const Eigen::Vector3d& center) {
+    R_ = R * R_;
+    center_ = R * (center_ - center) + center;
+    return *this;
+}
+
+double OrientedBoundingEllipsoid::Volume() const {
+    return 4 * M_PI * radii_(0) * radii_(1) * radii_(2) / 3.0;
+}
+
+std::vector<Eigen::Vector3d> OrientedBoundingEllipsoid::GetEllipsoidPoints()
+        const {
+    Eigen::Vector3d x_axis = R_ * Eigen::Vector3d(radii_(0), 0, 0);
+    Eigen::Vector3d y_axis = R_ * Eigen::Vector3d(0, radii_(1), 0);
+    Eigen::Vector3d z_axis = R_ * Eigen::Vector3d(0, 0, radii_(2));
+    std::vector<Eigen::Vector3d> points(6);
+    points[0] = center_ + R_ * x_axis;
+    points[1] = center_ - R_ * x_axis;
+    points[2] = center_ + y_axis;
+    points[3] = center_ - y_axis;
+    points[4] = center_ + z_axis;
+    points[5] = center_ - z_axis;
+    return points;
+}
+
+OrientedBoundingEllipsoid OrientedBoundingEllipsoid::CreateFromPoints(
+        const std::vector<Eigen::Vector3d>& points, bool robust) {
+    auto tpoints = core::eigen_converter::EigenVector3dVectorToTensor(
+            points, core::Float64, core::Device());
+    return t::geometry::kernel::minimum_obe::ComputeMinimumOBEKhachiyan(tpoints,
+                                                                        robust)
+            .ToLegacy();
+}
+
 OrientedBoundingBox& OrientedBoundingBox::Clear() {
     center_.setZero();
     extent_.setZero();
@@ -55,6 +159,11 @@ OrientedBoundingBox OrientedBoundingBox::GetMinimalOrientedBoundingBox(
     return *this;
 }
 
+OrientedBoundingEllipsoid OrientedBoundingBox::GetOrientedBoundingEllipsoid(
+        bool) const {
+    return OrientedBoundingEllipsoid::CreateFromPoints(GetBoxPoints());
+}
+
 OrientedBoundingBox& OrientedBoundingBox::Transform(
         const Eigen::Matrix4d& transformation) {
     utility::LogError(
@@ -233,6 +342,11 @@ OrientedBoundingBox AxisAlignedBoundingBox::GetMinimalOrientedBoundingBox(
     return OrientedBoundingBox::CreateFromAxisAlignedBoundingBox(*this);
 }
 
+OrientedBoundingEllipsoid AxisAlignedBoundingBox::GetOrientedBoundingEllipsoid(
+        bool) const {
+    return OrientedBoundingEllipsoid::CreateFromPoints(GetBoxPoints());
+}
+
 AxisAlignedBoundingBox::AxisAlignedBoundingBox(const Eigen::Vector3d& min_bound,
                                                const Eigen::Vector3d& max_bound)
     : Geometry3D(Geometry::GeometryType::AxisAlignedBoundingBox),
@@ -363,4 +477,4 @@ std::vector<size_t> AxisAlignedBoundingBox::GetPointIndicesWithinBoundingBox(
 }
 
 }  // namespace geometry
-}  // namespace open3d
+}  // namespace open3d

cpp/open3d/geometry/BoundingVolume.h

@@ -15,6 +15,122 @@ namespace open3d {
 namespace geometry {
 
 class AxisAlignedBoundingBox;
+class OrientedBoundingBox;
+
+class OrientedBoundingEllipsoid : public Geometry3D {
+public:
+    /// \brief Default constructor.
+    ///
+    /// Creates an empty Oriented Bounding Ellipsoid.
+    OrientedBoundingEllipsoid()
+        : Geometry3D(Geometry::GeometryType::OrientedBoundingEllipsoid),
+          center_(0, 0, 0),
+          R_(Eigen::Matrix3d::Identity()),
+          radii_(0, 0, 0),
+          color_(1, 1, 1) {}
+
+    /// \brief Parameterized constructor.
+    ///
+    /// \param center Specifies the center position of the bounding ellipsoid.
+    /// \param R The rotation matrix specifying the orientation of the
+    /// bounding ellipsoid with the original frame of reference.
+    /// \param radii The radii of the bounding ellipsoid.
+    OrientedBoundingEllipsoid(const Eigen::Vector3d& center,
+                              const Eigen::Matrix3d& R,
+                              const Eigen::Vector3d& radii)
+        : Geometry3D(Geometry::GeometryType::OrientedBoundingEllipsoid),
+          center_(center),
+          R_(R),
+          radii_(radii),
+          color_(1, 1, 1) {}
+    ~OrientedBoundingEllipsoid() override {}
+
+public:
+    OrientedBoundingEllipsoid& Clear() override;
+    bool IsEmpty() const override;
+    virtual Eigen::Vector3d GetMinBound() const override;
+    virtual Eigen::Vector3d GetMaxBound() const override;
+    virtual Eigen::Vector3d GetCenter() const override;
+
+    /// Creates an axis-aligned bounding box around the object.
+    virtual AxisAlignedBoundingBox GetAxisAlignedBoundingBox() const override;
+
+    /// Returns an oriented bounding box around the ellipsoid.
+    virtual OrientedBoundingBox GetOrientedBoundingBox(
+            bool robust) const override;
+
+    /// Returns an oriented bounding box around the ellipsoid.
+    virtual OrientedBoundingBox GetMinimalOrientedBoundingBox(
+            bool robust) const override;
+
+    /// Returns the object itself
+    virtual OrientedBoundingEllipsoid GetOrientedBoundingEllipsoid(
+            bool robust) const override;
+
+    virtual OrientedBoundingEllipsoid& Transform(
+            const Eigen::Matrix4d& transformation) override;
+    virtual OrientedBoundingEllipsoid& Translate(
+            const Eigen::Vector3d& translation, bool relative = true) override;
+    virtual OrientedBoundingEllipsoid& Scale(
+            const double scale, const Eigen::Vector3d& center) override;
+    virtual OrientedBoundingEllipsoid& Rotate(
+            const Eigen::Matrix3d& R, const Eigen::Vector3d& center) override;
+
+    /// Returns the volume of the bounding box.
+    double Volume() const;
+
+    /// Returns the six critical points of the bounding ellipsoid.
+    /// \verbatim
+    ///      ------- x
+    ///     /|
+    ///    / |
+    ///   /  | z
+    ///  y
+    ///                            2
+    ///                         .--|---.
+    ///                    .--'    |     '--.
+    ///               .--'         |          '--.
+    ///            .'              |   4           '.
+    ///           /                |  /              \
+    ///          /                 | /                \
+    ///       0 |------------------|-------------------| 1
+    ///          \               / |                  /
+    ///           \             /  |                 /
+    ///            '.         5    |               .'
+    ///               '--.         |          .--'
+    ///                    '--.    |     .--'
+    ///                         '--|---'
+    ///                            3
+    /// \endverbatim
+    std::vector<Eigen::Vector3d> GetEllipsoidPoints() const;
+
+    /// Return indices to points that are within the bounding ellipsoid.
+    std::vector<size_t> GetPointIndicesWithinBoundingEllipsoid(
+            const std::vector<Eigen::Vector3d>& points) const;
+
+    /// Creates an oriented bounding ellipsoid using a PCA.
+    /// Note, that this is only an approximation to the minimum oriented
+    /// bounding box that could be computed for example with O'Rourke's
+    /// algorithm (cf. http://cs.smith.edu/~jorourke/Papers/MinVolBox.pdf,
+    /// https://www.geometrictools.com/Documentation/MinimumVolumeBox.pdf)
+    /// \param points The input points
+    /// \param robust If set to true uses a more robust method which works
+    ///               in degenerate cases but introduces noise to the points
+    ///               coordinates.
+    static OrientedBoundingEllipsoid CreateFromPoints(
+            const std::vector<Eigen::Vector3d>& points, bool robust = false);
+
+public:
+    /// The center point of the bounding ellipsoid.
+    Eigen::Vector3d center_;
+    /// The rotation matrix of the bounding ellipsoid to transform the original
+    /// frame of reference to the frame of this ellipsoid.
+    Eigen::Matrix3d R_;
+    /// The radii of the bounding ellipsoid in its frame of reference.
+    Eigen::Vector3d radii_;
+    /// The color of the bounding ellipsoid in RGB.
+    Eigen::Vector3d color_;
+};
 
 /// \class OrientedBoundingBox
 ///
@@ -67,6 +183,10 @@ class OrientedBoundingBox : public Geometry3D {
     virtual OrientedBoundingBox GetMinimalOrientedBoundingBox(
             bool robust) const override;
 
+    /// Returns an oriented bounding ellipsoid encapsulating the object
+    virtual OrientedBoundingEllipsoid GetOrientedBoundingEllipsoid(
+            bool robust) const override;
+
     virtual OrientedBoundingBox& Transform(
             const Eigen::Matrix4d& transformation) override;
     virtual OrientedBoundingBox& Translate(const Eigen::Vector3d& translation,
@@ -206,6 +326,11 @@ class AxisAlignedBoundingBox : public Geometry3D {
     /// and orientation as the object
     virtual OrientedBoundingBox GetMinimalOrientedBoundingBox(
             bool robust = false) const override;
+
+    /// Returns an oriented bounding ellipsoid encapsulating the object
+    virtual OrientedBoundingEllipsoid GetOrientedBoundingEllipsoid(
+            bool robust) const override;
+
     virtual AxisAlignedBoundingBox& Transform(
             const Eigen::Matrix4d& transformation) override;
     virtual AxisAlignedBoundingBox& Translate(
@@ -288,4 +413,4 @@ class AxisAlignedBoundingBox : public Geometry3D {
 };
 
 }  // namespace geometry
-}  // namespace open3d
+}  // namespace open3d

cpp/open3d/geometry/Geometry.h

@@ -47,6 +47,8 @@ class Geometry {
         OrientedBoundingBox = 11,
         /// AxisAlignedBoundingBox
         AxisAlignedBoundingBox = 12,
+        /// OrientedBoundingEllipsoid
+        OrientedBoundingEllipsoid = 13,
     };
 
 public:

cpp/open3d/geometry/Geometry3D.h

@@ -19,6 +19,7 @@ namespace geometry {
 
 class AxisAlignedBoundingBox;
 class OrientedBoundingBox;
+class OrientedBoundingEllipsoid;
 
 /// \class Geometry3D
 ///
@@ -66,6 +67,14 @@ class Geometry3D : public Geometry {
     virtual OrientedBoundingBox GetMinimalOrientedBoundingBox(
             bool robust = false) const = 0;
 
+    /// Creates an oriented bounding ellipsoid around the points of the object.
+    /// Further details in OrientedBoundingEllipsoid::CreateFromPoints()
+    /// \param robust If set to true uses a more robust method which works
+    ///               in degenerate cases but introduces noise to the points
+    ///               coordinates.
+    virtual OrientedBoundingEllipsoid GetOrientedBoundingEllipsoid(
+            bool robust = false) const = 0;
+
     /// \brief Apply transformation (4x4 matrix) to the geometry coordinates.
     virtual Geometry3D& Transform(const Eigen::Matrix4d& transformation) = 0;
 

cpp/open3d/geometry/LineSet.cpp

@@ -45,6 +45,11 @@ OrientedBoundingBox LineSet::GetMinimalOrientedBoundingBox(bool robust) const {
     return OrientedBoundingBox::CreateFromPointsMinimal(points_, robust);
 }
 
+OrientedBoundingEllipsoid LineSet::GetOrientedBoundingEllipsoid(
+        bool robust) const {
+    return OrientedBoundingEllipsoid::CreateFromPoints(points_, robust);
+}
+
 LineSet &LineSet::Transform(const Eigen::Matrix4d &transformation) {
     TransformPoints(transformation, points_);
     return *this;

cpp/open3d/geometry/LineSet.h

@@ -17,6 +17,7 @@ namespace open3d {
 namespace geometry {
 
 class PointCloud;
+class OrientedBoundingEllipsoid;
 class OrientedBoundingBox;
 class AxisAlignedBoundingBox;
 class TriangleMesh;
@@ -71,6 +72,14 @@ class LineSet : public Geometry3D {
     OrientedBoundingBox GetMinimalOrientedBoundingBox(
             bool robust = false) const override;
 
+    /// Creates an oriented bounding ellipsoid around the points of the object.
+    /// Further details in OrientedBoundingEllipsoid::CreateFromPoints()
+    /// \param robust If set to true uses a more robust method which works
+    ///               in degenerate cases but introduces noise to the points
+    ///               coordinates.
+    OrientedBoundingEllipsoid GetOrientedBoundingEllipsoid(
+            bool robust = false) const override;
+
     LineSet &Transform(const Eigen::Matrix4d &transformation) override;
     LineSet &Translate(const Eigen::Vector3d &translation,
                        bool relative = true) override;
@@ -133,6 +142,13 @@ class LineSet : public Geometry3D {
     static std::shared_ptr<LineSet> CreateFromAxisAlignedBoundingBox(
             const AxisAlignedBoundingBox &box);
 
+    /// \brief Factory function to create a LineSet from an
+    /// OrientedBoundingEllipsoid.
+    ///
+    /// \param box The input bounding box.
+    static std::shared_ptr<LineSet> CreateFromOrientedBoundingEllipsoid(
+            const OrientedBoundingEllipsoid &ellipsoid);
+
     /// Factory function to create a LineSet from edges of a triangle mesh.
     ///
     /// \param mesh The input triangle mesh.