diff --git a/cpp/open3d/geometry/BoundingVolume.cpp b/cpp/open3d/geometry/BoundingVolume.cpp index 410e2ac8a1b..c0bcdd6f992 100644 --- a/cpp/open3d/geometry/BoundingVolume.cpp +++ b/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 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 points(6); + points[0] = center_ + x_axis; + points[1] = center_ - 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& 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 AxisAlignedBoundingBox::GetPointIndicesWithinBoundingBox( } } // namespace geometry -} // namespace open3d +} // namespace open3d \ No newline at end of file diff --git a/cpp/open3d/geometry/BoundingVolume.h b/cpp/open3d/geometry/BoundingVolume.h index 817ec0d36a4..e81fee0dad5 100644 --- a/cpp/open3d/geometry/BoundingVolume.h +++ b/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 GetEllipsoidPoints() const; + + /// Return indices to points that are within the bounding ellipsoid. + std::vector GetPointIndicesWithinBoundingEllipsoid( + const std::vector& 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& 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 \ No newline at end of file diff --git a/cpp/open3d/geometry/Geometry.h b/cpp/open3d/geometry/Geometry.h index 4ddf79bcc0c..4bcffd71ea4 100644 --- a/cpp/open3d/geometry/Geometry.h +++ b/cpp/open3d/geometry/Geometry.h @@ -47,6 +47,8 @@ class Geometry { OrientedBoundingBox = 11, /// AxisAlignedBoundingBox AxisAlignedBoundingBox = 12, + /// OrientedBoundingEllipsoid + OrientedBoundingEllipsoid = 13, }; public: diff --git a/cpp/open3d/geometry/Geometry3D.h b/cpp/open3d/geometry/Geometry3D.h index 74a0cc887a3..9fec397d6e9 100644 --- a/cpp/open3d/geometry/Geometry3D.h +++ b/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; diff --git a/cpp/open3d/geometry/LineSet.cpp b/cpp/open3d/geometry/LineSet.cpp index 9a38d2a7371..19d5cd41c34 100644 --- a/cpp/open3d/geometry/LineSet.cpp +++ b/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; diff --git a/cpp/open3d/geometry/LineSet.h b/cpp/open3d/geometry/LineSet.h index 3718b6d446c..3f9424e5ce6 100644 --- a/cpp/open3d/geometry/LineSet.h +++ b/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 CreateFromAxisAlignedBoundingBox( const AxisAlignedBoundingBox &box); + /// \brief Factory function to create a LineSet from an + /// OrientedBoundingEllipsoid. + /// + /// \param ellipsoid The input bounding ellipsoid. + static std::shared_ptr CreateFromOrientedBoundingEllipsoid( + const OrientedBoundingEllipsoid &ellipsoid); + /// Factory function to create a LineSet from edges of a triangle mesh. /// /// \param mesh The input triangle mesh. diff --git a/cpp/open3d/geometry/LineSetFactory.cpp b/cpp/open3d/geometry/LineSetFactory.cpp index 029fdd9d1bb..8c31b9bb439 100644 --- a/cpp/open3d/geometry/LineSetFactory.cpp +++ b/cpp/open3d/geometry/LineSetFactory.cpp @@ -17,9 +17,9 @@ namespace open3d { namespace geometry { std::shared_ptr LineSet::CreateFromPointCloudCorrespondences( - const PointCloud &cloud0, - const PointCloud &cloud1, - const std::vector> &correspondences) { + const PointCloud& cloud0, + const PointCloud& cloud1, + const std::vector>& correspondences) { auto lineset_ptr = std::make_shared(); size_t point0_size = cloud0.points_.size(); size_t point1_size = cloud1.points_.size(); @@ -39,7 +39,7 @@ std::shared_ptr LineSet::CreateFromPointCloudCorrespondences( } std::shared_ptr LineSet::CreateFromTriangleMesh( - const TriangleMesh &mesh) { + const TriangleMesh& mesh) { auto line_set = std::make_shared(); line_set->points_ = mesh.vertices_; @@ -52,7 +52,7 @@ std::shared_ptr LineSet::CreateFromTriangleMesh( line_set->lines_.push_back(Eigen::Vector2i(vidx0, vidx1)); } }; - for (const auto &triangle : mesh.triangles_) { + for (const auto& triangle : mesh.triangles_) { InsertEdge(triangle(0), triangle(1)); InsertEdge(triangle(1), triangle(2)); InsertEdge(triangle(2), triangle(0)); @@ -61,8 +61,21 @@ std::shared_ptr LineSet::CreateFromTriangleMesh( return line_set; } +std::shared_ptr LineSet::CreateFromOrientedBoundingEllipsoid( + const OrientedBoundingEllipsoid& ellipsoid) { + std::shared_ptr obel = + geometry::TriangleMesh::CreateEllipsoid(ellipsoid.radii_(0), + ellipsoid.radii_(1), + ellipsoid.radii_(2)); + obel->Rotate(ellipsoid.R_, Eigen::Vector3d::Zero()); + obel->Translate(ellipsoid.center_); + auto line_set = CreateFromTriangleMesh(*obel); + line_set->PaintUniformColor(ellipsoid.color_); + return line_set; +} + std::shared_ptr LineSet::CreateFromOrientedBoundingBox( - const OrientedBoundingBox &box) { + const OrientedBoundingBox& box) { auto line_set = std::make_shared(); line_set->points_ = box.GetBoxPoints(); line_set->lines_.push_back(Eigen::Vector2i(0, 1)); @@ -82,7 +95,7 @@ std::shared_ptr LineSet::CreateFromOrientedBoundingBox( } std::shared_ptr LineSet::CreateFromAxisAlignedBoundingBox( - const AxisAlignedBoundingBox &box) { + const AxisAlignedBoundingBox& box) { auto line_set = std::make_shared(); line_set->points_ = box.GetBoxPoints(); line_set->lines_.push_back(Eigen::Vector2i(0, 1)); @@ -101,7 +114,7 @@ std::shared_ptr LineSet::CreateFromAxisAlignedBoundingBox( return line_set; } -std::shared_ptr LineSet::CreateFromTetraMesh(const TetraMesh &mesh) { +std::shared_ptr LineSet::CreateFromTetraMesh(const TetraMesh& mesh) { auto line_set = std::make_shared(); line_set->points_ = mesh.vertices_; @@ -114,7 +127,7 @@ std::shared_ptr LineSet::CreateFromTetraMesh(const TetraMesh &mesh) { line_set->lines_.push_back(Eigen::Vector2i(vidx0, vidx1)); } }; - for (const auto &tetra : mesh.tetras_) { + for (const auto& tetra : mesh.tetras_) { InsertEdge(tetra(0), tetra(1)); InsertEdge(tetra(1), tetra(2)); InsertEdge(tetra(2), tetra(0)); @@ -129,8 +142,8 @@ std::shared_ptr LineSet::CreateFromTetraMesh(const TetraMesh &mesh) { std::shared_ptr LineSet::CreateCameraVisualization( int view_width_px, int view_height_px, - const Eigen::Matrix3d &intrinsic, - const Eigen::Matrix4d &extrinsic, + const Eigen::Matrix3d& intrinsic, + const Eigen::Matrix4d& extrinsic, double scale) { Eigen::Matrix4d intrinsic4; intrinsic4 << intrinsic(0, 0), intrinsic(0, 1), intrinsic(0, 2), 0.0, @@ -140,8 +153,8 @@ std::shared_ptr LineSet::CreateCameraVisualization( Eigen::Matrix4d m = (intrinsic4 * extrinsic).inverse(); auto lines = std::make_shared(); - auto mult = [](const Eigen::Matrix4d &m, - const Eigen::Vector3d &v) -> Eigen::Vector3d { + auto mult = [](const Eigen::Matrix4d& m, + const Eigen::Vector3d& v) -> Eigen::Vector3d { Eigen::Vector4d v4(v.x(), v.y(), v.z(), 1.0); auto result = m * v4; return Eigen::Vector3d{result.x() / result.w(), result.y() / result.w(), diff --git a/cpp/open3d/geometry/MeshBase.cpp b/cpp/open3d/geometry/MeshBase.cpp index 3d5714d9072..31b4c3b5e38 100644 --- a/cpp/open3d/geometry/MeshBase.cpp +++ b/cpp/open3d/geometry/MeshBase.cpp @@ -54,6 +54,11 @@ OrientedBoundingBox MeshBase::GetMinimalOrientedBoundingBox(bool robust) const { return OrientedBoundingBox::CreateFromPointsMinimal(vertices_, robust); } +OrientedBoundingEllipsoid MeshBase::GetOrientedBoundingEllipsoid( + bool robust) const { + return OrientedBoundingEllipsoid::CreateFromPoints(vertices_, robust); +} + MeshBase &MeshBase::Transform(const Eigen::Matrix4d &transformation) { TransformPoints(transformation, vertices_); TransformNormals(transformation, vertex_normals_); diff --git a/cpp/open3d/geometry/MeshBase.h b/cpp/open3d/geometry/MeshBase.h index 2a5793ac62b..d098babcb2a 100644 --- a/cpp/open3d/geometry/MeshBase.h +++ b/cpp/open3d/geometry/MeshBase.h @@ -22,6 +22,7 @@ namespace geometry { class PointCloud; class TriangleMesh; +class OrientedBoundingEllipsoid; /// \class MeshBase /// @@ -88,6 +89,14 @@ class MeshBase : public Geometry3D { virtual 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. + virtual OrientedBoundingEllipsoid GetOrientedBoundingEllipsoid( + bool robust = false) const override; + virtual MeshBase &Transform(const Eigen::Matrix4d &transformation) override; virtual MeshBase &Translate(const Eigen::Vector3d &translation, bool relative = true) override; diff --git a/cpp/open3d/geometry/Octree.cpp b/cpp/open3d/geometry/Octree.cpp index 23efcb06e81..dfa882d6a7b 100644 --- a/cpp/open3d/geometry/Octree.cpp +++ b/cpp/open3d/geometry/Octree.cpp @@ -520,6 +520,12 @@ OrientedBoundingBox Octree::GetMinimalOrientedBoundingBox(bool robust) const { GetAxisAlignedBoundingBox()); } +OrientedBoundingEllipsoid Octree::GetOrientedBoundingEllipsoid( + bool robust) const { + return OrientedBoundingEllipsoid::CreateFromPoints( + GetAxisAlignedBoundingBox().GetBoxPoints()); +} + Octree& Octree::Transform(const Eigen::Matrix4d& transformation) { utility::LogError("Not implemented"); return *this; diff --git a/cpp/open3d/geometry/Octree.h b/cpp/open3d/geometry/Octree.h index 5b3b6d90c52..a518eb8a75e 100644 --- a/cpp/open3d/geometry/Octree.h +++ b/cpp/open3d/geometry/Octree.h @@ -293,6 +293,14 @@ class Octree : public Geometry3D, public utility::IJsonConvertible { 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; + Octree& Transform(const Eigen::Matrix4d& transformation) override; Octree& Translate(const Eigen::Vector3d& translation, bool relative = true) override; diff --git a/cpp/open3d/geometry/PointCloud.cpp b/cpp/open3d/geometry/PointCloud.cpp index b9ecab11f82..e8d275220b4 100644 --- a/cpp/open3d/geometry/PointCloud.cpp +++ b/cpp/open3d/geometry/PointCloud.cpp @@ -57,6 +57,11 @@ OrientedBoundingBox PointCloud::GetMinimalOrientedBoundingBox( return OrientedBoundingBox::CreateFromPointsMinimal(points_, robust); } +OrientedBoundingEllipsoid PointCloud::GetOrientedBoundingEllipsoid( + bool robust) const { + return OrientedBoundingEllipsoid::CreateFromPoints(points_, robust); +} + PointCloud &PointCloud::Transform(const Eigen::Matrix4d &transformation) { TransformPoints(transformation, points_); TransformNormals(transformation, normals_); diff --git a/cpp/open3d/geometry/PointCloud.h b/cpp/open3d/geometry/PointCloud.h index 3909e68ce1b..4e003605b45 100644 --- a/cpp/open3d/geometry/PointCloud.h +++ b/cpp/open3d/geometry/PointCloud.h @@ -55,6 +55,8 @@ class PointCloud : public Geometry3D { bool robust = false) const override; OrientedBoundingBox GetMinimalOrientedBoundingBox( bool robust = false) const override; + OrientedBoundingEllipsoid GetOrientedBoundingEllipsoid( + bool robust = false) const override; PointCloud &Transform(const Eigen::Matrix4d &transformation) override; PointCloud &Translate(const Eigen::Vector3d &translation, bool relative = true) override; diff --git a/cpp/open3d/geometry/TriangleMesh.cpp b/cpp/open3d/geometry/TriangleMesh.cpp index bb5f9571e09..10a5e4b0310 100644 --- a/cpp/open3d/geometry/TriangleMesh.cpp +++ b/cpp/open3d/geometry/TriangleMesh.cpp @@ -1692,7 +1692,7 @@ std::shared_ptr TriangleMesh::Crop( const OrientedBoundingBox &bbox) const { if (bbox.IsEmpty()) { utility::LogError( - "AxisAlignedBoundingBox either has zeros " + "OrientedBoundingBox either has zeros " "size, or has wrong bounds."); return std::make_shared(); } diff --git a/cpp/open3d/geometry/TriangleMesh.h b/cpp/open3d/geometry/TriangleMesh.h index 7f0b05c6ca5..d8661da8d77 100644 --- a/cpp/open3d/geometry/TriangleMesh.h +++ b/cpp/open3d/geometry/TriangleMesh.h @@ -589,6 +589,18 @@ class TriangleMesh : public MeshBase { const Eigen::Vector3d &scale = Eigen::Vector3d::Ones(), bool create_uv_map = false); + /// Factory function to create solid mesh from an OrientedBoundingEllipsoid. + /// \param obel OrientedBoundingEllipsoid object to create a mesh of + /// \param scale scale factor along each direction of + /// OrientedBoundingEllipsoid + /// \param resolution defines the resolution of the ellipsoid. + /// \param create_uv_map add default UV map to the mesh. + static std::shared_ptr CreateFromOrientedBoundingEllipsoid( + const OrientedBoundingEllipsoid &obel, + const Eigen::Vector3d &scale = Eigen::Vector3d::Ones(), + int resolution = 20, + bool create_uv_map = false); + /// Factory function to create a box mesh (TriangleMeshFactory.cpp) /// The left bottom corner on the front will be placed at (0, 0, 0). /// \param width is x-directional length. @@ -619,6 +631,20 @@ class TriangleMesh : public MeshBase { int resolution = 20, bool create_uv_map = false); + /// Factory function to create an ellipsoid mesh (TriangleMeshFactory.cpp) + /// The ellipsoid will be centered at (0, 0, 0). + /// \param radius_x defines first radii of the ellipsoid. + /// \param radius_y defines second radii of the ellipsoid. + /// \param radius_z defines third radii of the ellipsoid. + /// \param resolution defines the resolution of the ellipsoid. + /// \param create_uv_map add default UV map to the mesh. + static std::shared_ptr CreateEllipsoid( + double radius_x = 1.0, + double radius_y = 1.0, + double radius_z = 1.0, + int resolution = 20, + bool create_uv_map = false); + /// Factory function to create a cylinder mesh (TriangleMeshFactory.cpp) /// The axis of the cylinder will be from (0, 0, -height/2) to (0, 0, /// height/2). The circle with radius will be split into diff --git a/cpp/open3d/geometry/TriangleMeshFactory.cpp b/cpp/open3d/geometry/TriangleMeshFactory.cpp index 68341245bf4..54e6be79d56 100644 --- a/cpp/open3d/geometry/TriangleMeshFactory.cpp +++ b/cpp/open3d/geometry/TriangleMeshFactory.cpp @@ -147,7 +147,22 @@ std::shared_ptr TriangleMesh::CreateFromOrientedBoundingBox( auto mesh = CreateBox(origin.x(), origin.y(), origin.z(), create_uv_map); mesh->Rotate(obox.R_, origin / 2.); mesh->Translate(obox.center_ - origin / 2.); - mesh->PaintUniformColor(obox.color_); + return mesh; +} + +std::shared_ptr TriangleMesh::CreateFromOrientedBoundingEllipsoid( + const OrientedBoundingEllipsoid &obel, + const Eigen::Vector3d &scale /*= Eigen::Vector3d::Ones()*/, + int resolution /* = 20 */, + bool create_uv_map /*= false*/) { + Eigen::Vector3d origin = scale.asDiagonal() * obel.radii_; + auto mesh = CreateEllipsoid(origin.x(), origin.y(), origin.z(), resolution, + create_uv_map); + Eigen::Matrix4d t = Eigen::Matrix4d::Identity(); + t.block<3, 3>(0, 0) = obel.R_; + t.block<3, 1>(0, 3) = obel.center_; + mesh->Transform(t); + mesh->PaintUniformColor(obel.color_); return mesh; } @@ -380,6 +395,232 @@ std::shared_ptr TriangleMesh::CreateSphere( return mesh; } +std::shared_ptr TriangleMesh::CreateEllipsoid( + double radius_x /* = 1.0 */, + double radius_y /* = 1.0 */, + double radius_z /* = 1.0 */, + int resolution /* = 20 */, + bool create_uv_map /* = false*/) { + auto mesh = std::make_shared(); + + // Basic parameter checks + if (radius_x <= 0) { + utility::LogError("radius_x must be > 0, but got {}", radius_x); + } + if (radius_y <= 0) { + utility::LogError("radius_y must be > 0, but got {}", radius_y); + } + if (radius_z <= 0) { + utility::LogError("radius_z must be > 0, but got {}", radius_z); + } + if (resolution <= 0) { + utility::LogError("resolution <= 0"); + } + + // We use the same vertex count formula as the sphere: + // 2 poles + (resolution - 1) "rings" * (2 * resolution) points per ring + mesh->vertices_.resize(2 * resolution * (resolution - 1) + 2); + + // Maps for UV coordinates (only used if create_uv_map == true) + std::unordered_map> map_vertices_to_uv; + std::unordered_map> + map_cut_vertices_to_uv; + + // Define the top and bottom poles + // Top pole at (0, 0, +radius_z), bottom at (0, 0, -radius_z) + mesh->vertices_[0] = Eigen::Vector3d(0.0, 0.0, radius_z); + mesh->vertices_[1] = Eigen::Vector3d(0.0, 0.0, -radius_z); + + // Generate intermediate "rings" between the poles + double step = M_PI / (double)resolution; // step for polar angle + for (int i = 1; i < resolution; i++) { + double alpha = step * i; // polar angle + double uv_row = (1.0 / resolution) * i; + int base = 2 + 2 * resolution * (i - 1); + + for (int j = 0; j < 2 * resolution; j++) { + double theta = step * j; // azimuthal angle + double uv_col = (1.0 / (2.0 * resolution)) * j; + + // Similar to sphere but scaled by radius_x, radius_y, radius_z + double x = std::sin(alpha) * std::cos(theta) * radius_x; + double y = std::sin(alpha) * std::sin(theta) * radius_y; + double z = std::cos(alpha) * radius_z; + + mesh->vertices_[base + j] = Eigen::Vector3d(x, y, z); + + // Store UV if requested + if (create_uv_map) { + map_vertices_to_uv[base + j] = std::make_pair(uv_row, uv_col); + } + } + + // We store the "cut" vertex to handle the seam in the UV map + if (create_uv_map) { + // The first vertex in each ring is assigned a UV of (row, 1.0) as + // the seam + map_cut_vertices_to_uv[base] = std::make_pair(uv_row, 1.0); + } + } + + //---------------------------------------------------------------------- + // Now we create the triangles. The topology is identical to the sphere: + // 1) Triangles connecting the top pole + // 2) Triangles connecting the bottom pole + // 3) Triangles connecting each intermediate ring + //---------------------------------------------------------------------- + + // + // 1) Triangles for poles + // + for (int j = 0; j < 2 * resolution; j++) { + int j1 = (j + 1) % (2 * resolution); + + // Connect top pole (index 0) + int base_top = 2; + mesh->triangles_.push_back( + Eigen::Vector3i(0, base_top + j, base_top + j1)); + + // Connect bottom pole (index 1) + int base_bottom = 2 + 2 * resolution * (resolution - 2); + mesh->triangles_.push_back( + Eigen::Vector3i(1, base_bottom + j1, base_bottom + j)); + } + + // 1a) UV coordinates for poles + if (create_uv_map) { + for (int j = 0; j < 2 * resolution - 1; j++) { + int j1 = (j + 1) % (2 * resolution); + int base_top = 2; + double width = 1.0 / (2.0 * resolution); + double base_offset = width / 2.0; + double uv_col = base_offset + width * j; + + // Triangle for top pole + mesh->triangle_uvs_.push_back(Eigen::Vector2d(0.0, uv_col)); + mesh->triangle_uvs_.push_back( + Eigen::Vector2d(map_vertices_to_uv[base_top + j].first, + map_vertices_to_uv[base_top + j].second)); + mesh->triangle_uvs_.push_back( + Eigen::Vector2d(map_vertices_to_uv[base_top + j1].first, + map_vertices_to_uv[base_top + j1].second)); + + // Triangle for bottom pole + int base_bottom = 2 + 2 * resolution * (resolution - 2); + mesh->triangle_uvs_.push_back(Eigen::Vector2d(1.0, uv_col)); + mesh->triangle_uvs_.push_back(Eigen::Vector2d( + map_vertices_to_uv[base_bottom + j1].first, + map_vertices_to_uv[base_bottom + j1].second)); + mesh->triangle_uvs_.push_back(Eigen::Vector2d( + map_vertices_to_uv[base_bottom + j].first, + map_vertices_to_uv[base_bottom + j].second)); + } + + // Handle the seam case (cut vertices) for the pole triangles + int j = 2 * resolution - 1; + double width = 1.0 / (2.0 * resolution); + double base_offset = width / 2.0; + double uv_col = base_offset + width * j; + + // top pole with cut seam + { + int base_top = 2; + mesh->triangle_uvs_.push_back(Eigen::Vector2d(0.0, uv_col)); + mesh->triangle_uvs_.push_back( + Eigen::Vector2d(map_vertices_to_uv[base_top + j].first, + map_vertices_to_uv[base_top + j].second)); + mesh->triangle_uvs_.push_back( + Eigen::Vector2d(map_cut_vertices_to_uv[base_top].first, + map_cut_vertices_to_uv[base_top].second)); + } + // bottom pole with cut seam + { + int base_bottom = 2 + 2 * resolution * (resolution - 2); + mesh->triangle_uvs_.push_back(Eigen::Vector2d(1.0, uv_col)); + mesh->triangle_uvs_.push_back(Eigen::Vector2d( + map_cut_vertices_to_uv[base_bottom].first, + map_cut_vertices_to_uv[base_bottom].second)); + mesh->triangle_uvs_.push_back(Eigen::Vector2d( + map_vertices_to_uv[base_bottom + j].first, + map_vertices_to_uv[base_bottom + j].second)); + } + } + + // + // 2) Triangles for non-polar region + // + for (int i = 1; i < resolution - 1; i++) { + int base1 = 2 + 2 * resolution * (i - 1); + int base2 = base1 + 2 * resolution; + + for (int j = 0; j < 2 * resolution; j++) { + int j1 = (j + 1) % (2 * resolution); + // Two triangles per quad + mesh->triangles_.push_back( + Eigen::Vector3i(base2 + j, base1 + j1, base1 + j)); + mesh->triangles_.push_back( + Eigen::Vector3i(base2 + j, base2 + j1, base1 + j1)); + } + } + + // UV mapping for the non-polar region + if (create_uv_map) { + for (int i = 1; i < resolution - 1; i++) { + int base1 = 2 + 2 * resolution * (i - 1); + int base2 = base1 + 2 * resolution; + + for (int j = 0; j < 2 * resolution - 1; j++) { + int j1 = (j + 1) % (2 * resolution); + + // Upper triangle + mesh->triangle_uvs_.push_back( + Eigen::Vector2d(map_vertices_to_uv[base2 + j].first, + map_vertices_to_uv[base2 + j].second)); + mesh->triangle_uvs_.push_back( + Eigen::Vector2d(map_vertices_to_uv[base1 + j1].first, + map_vertices_to_uv[base1 + j1].second)); + mesh->triangle_uvs_.push_back( + Eigen::Vector2d(map_vertices_to_uv[base1 + j].first, + map_vertices_to_uv[base1 + j].second)); + + // Lower triangle + mesh->triangle_uvs_.push_back( + Eigen::Vector2d(map_vertices_to_uv[base2 + j].first, + map_vertices_to_uv[base2 + j].second)); + mesh->triangle_uvs_.push_back( + Eigen::Vector2d(map_vertices_to_uv[base2 + j1].first, + map_vertices_to_uv[base2 + j1].second)); + mesh->triangle_uvs_.push_back( + Eigen::Vector2d(map_vertices_to_uv[base1 + j1].first, + map_vertices_to_uv[base1 + j1].second)); + } + + // Handle seam for non-polar region + mesh->triangle_uvs_.push_back(Eigen::Vector2d( + map_vertices_to_uv[base2 + 2 * resolution - 1].first, + map_vertices_to_uv[base2 + 2 * resolution - 1].second)); + mesh->triangle_uvs_.push_back( + Eigen::Vector2d(map_cut_vertices_to_uv[base1].first, + map_cut_vertices_to_uv[base1].second)); + mesh->triangle_uvs_.push_back(Eigen::Vector2d( + map_vertices_to_uv[base1 + 2 * resolution - 1].first, + map_vertices_to_uv[base1 + 2 * resolution - 1].second)); + + mesh->triangle_uvs_.push_back(Eigen::Vector2d( + map_vertices_to_uv[base2 + 2 * resolution - 1].first, + map_vertices_to_uv[base2 + 2 * resolution - 1].second)); + mesh->triangle_uvs_.push_back( + Eigen::Vector2d(map_cut_vertices_to_uv[base2].first, + map_cut_vertices_to_uv[base2].second)); + mesh->triangle_uvs_.push_back( + Eigen::Vector2d(map_cut_vertices_to_uv[base1].first, + map_cut_vertices_to_uv[base1].second)); + } + } + + return mesh; +} + std::shared_ptr TriangleMesh::CreateCylinder( double radius /* = 1.0*/, double height /* = 2.0*/, diff --git a/cpp/open3d/geometry/VoxelGrid.cpp b/cpp/open3d/geometry/VoxelGrid.cpp index f4c92d71c5a..d2f76095d56 100644 --- a/cpp/open3d/geometry/VoxelGrid.cpp +++ b/cpp/open3d/geometry/VoxelGrid.cpp @@ -122,6 +122,11 @@ OrientedBoundingBox VoxelGrid::GetMinimalOrientedBoundingBox( robust); } +OrientedBoundingEllipsoid VoxelGrid::GetOrientedBoundingEllipsoid(bool) const { + return OrientedBoundingEllipsoid::CreateFromPoints( + GetAxisAlignedBoundingBox().GetBoxPoints()); +} + VoxelGrid &VoxelGrid::Transform(const Eigen::Matrix4d &transformation) { // Only update origin_ and rotation_ (lazy transform) origin_ = (transformation.block<3, 3>(0, 0) * origin_) + diff --git a/cpp/open3d/geometry/VoxelGrid.h b/cpp/open3d/geometry/VoxelGrid.h index 031dadbd104..7c04c48c6d4 100644 --- a/cpp/open3d/geometry/VoxelGrid.h +++ b/cpp/open3d/geometry/VoxelGrid.h @@ -91,6 +91,11 @@ class VoxelGrid : public Geometry3D { OrientedBoundingBox GetMinimalOrientedBoundingBox( bool robust = false) const override; + /// Creates an oriented bounding ellipsoid that encapsulates the + /// axis-aligned bounding from GetAxisAlignedBoundingBox(). + OrientedBoundingEllipsoid GetOrientedBoundingEllipsoid( + bool robust = false) const override; + VoxelGrid &Transform(const Eigen::Matrix4d &transformation) override; VoxelGrid &Translate(const Eigen::Vector3d &translation, bool relative = true) override; diff --git a/cpp/open3d/t/geometry/BoundingVolume.cpp b/cpp/open3d/t/geometry/BoundingVolume.cpp index 3ebfeeab5c9..f297fcb9fce 100644 --- a/cpp/open3d/t/geometry/BoundingVolume.cpp +++ b/cpp/open3d/t/geometry/BoundingVolume.cpp @@ -10,6 +10,7 @@ #include "open3d/core/EigenConverter.h" #include "open3d/core/TensorFunction.h" #include "open3d/t/geometry/kernel/MinimumOBB.h" +#include "open3d/t/geometry/kernel/MinimumOBE.h" #include "open3d/t/geometry/kernel/PointCloud.h" namespace open3d { @@ -595,6 +596,305 @@ OrientedBoundingBox OrientedBoundingBox::CreateFromPoints( } } +OrientedBoundingEllipsoid::OrientedBoundingEllipsoid(const core::Device &device) + : Geometry(Geometry::GeometryType::OrientedBoundingEllipsoid, 3), + device_(device), + dtype_(core::Float32), + center_(core::Tensor::Zeros({3}, dtype_, device)), + rotation_(core::Tensor::Eye(3, dtype_, device)), + radii_(core::Tensor::Zeros({3}, dtype_, device)), + color_(core::Tensor::Ones({3}, dtype_, device)) {} + +OrientedBoundingEllipsoid::OrientedBoundingEllipsoid( + const core::Tensor ¢er, + const core::Tensor &rotation, + const core::Tensor &radii) + : OrientedBoundingEllipsoid([&]() { + core::AssertTensorDevice(center, radii.GetDevice()); + core::AssertTensorDevice(rotation, radii.GetDevice()); + core::AssertTensorDtype(center, radii.GetDtype()); + core::AssertTensorDtype(rotation, radii.GetDtype()); + core::AssertTensorDtypes(radii, {core::Float32, core::Float64}); + core::AssertTensorShape(center, {3}); + core::AssertTensorShape(radii, {3}); + core::AssertTensorShape(rotation, {3, 3}); + return center.GetDevice(); + }()) { + device_ = center.GetDevice(); + dtype_ = center.GetDtype(); + + center_ = center; + radii_ = radii; + rotation_ = rotation; + color_ = core::Tensor::Ones({3}, dtype_, device_); + + // Check if the bounding ellipsoid is valid by checking the volume and the + // orthogonality of rotation. + if (Volume() < 0 || + !rotation_.T().AllClose(rotation.Inverse(), 1e-5, 1e-5)) { + utility::LogError( + "Invalid oriented bounding ellipsoid. Please make sure the " + "values of radii are all positive and the rotation matrix is " + "orthogonal."); + } +} + +OrientedBoundingEllipsoid OrientedBoundingEllipsoid::To( + const core::Device &device, bool copy) const { + if (!copy && GetDevice() == device) { + return *this; + } + OrientedBoundingEllipsoid ellipsoid(device); + ellipsoid.SetCenter(center_.To(device, true)); + ellipsoid.SetRotation(rotation_.To(device, true)); + ellipsoid.SetRadii(radii_.To(device, true)); + ellipsoid.SetColor(color_.To(device, true)); + return ellipsoid; +} + +OrientedBoundingEllipsoid &OrientedBoundingEllipsoid::Clear() { + center_ = core::Tensor::Zeros({3}, GetDtype(), GetDevice()); + radii_ = core::Tensor::Zeros({3}, GetDtype(), GetDevice()); + rotation_ = core::Tensor::Eye(3, GetDtype(), GetDevice()); + color_ = core::Tensor::Ones({3}, GetDtype(), GetDevice()); + return *this; +} + +void OrientedBoundingEllipsoid::SetCenter(const core::Tensor ¢er) { + core::AssertTensorDevice(center, GetDevice()); + core::AssertTensorShape(center, {3}); + core::AssertTensorDtypes(center, {core::Float32, core::Float64}); + + center_ = center.To(GetDtype()); +} + +void OrientedBoundingEllipsoid::SetRadii(const core::Tensor &radii) { + core::AssertTensorDevice(radii, GetDevice()); + core::AssertTensorShape(radii, {3}); + core::AssertTensorDtypes(radii, {core::Float32, core::Float64}); + + if (radii.Min({0}).To(core::Float64).Item() <= 0) { + utility::LogError( + "Invalid oriented bounding ellipsoid. Please make sure the " + "values of radii are all positive."); + } + + radii_ = radii.To(GetDtype()); +} + +void OrientedBoundingEllipsoid::SetRotation(const core::Tensor &rotation) { + core::AssertTensorDevice(rotation, GetDevice()); + core::AssertTensorShape(rotation, {3, 3}); + core::AssertTensorDtypes(rotation, {core::Float32, core::Float64}); + + if (!rotation.T().AllClose(rotation.Inverse(), 1e-5, 1e-5)) { + utility::LogWarning( + "Invalid oriented bounding ellipsoid. Please make sure the " + "rotation matrix is orthogonal."); + } else { + rotation_ = rotation.To(GetDtype()); + } +} + +void OrientedBoundingEllipsoid::SetColor(const core::Tensor &color) { + core::AssertTensorDevice(color, GetDevice()); + core::AssertTensorShape(color, {3}); + if (color.Max({0}).To(core::Float64).Item() > 1.0 || + color.Min({0}).To(core::Float64).Item() < 0.0) { + utility::LogError( + "The color must be in the range [0, 1], but found in range " + "[{}, {}].", + color.Min({0}).To(core::Float64).Item(), + color.Max({0}).To(core::Float64).Item()); + } + + color_ = color.To(GetDtype()); +} + +core::Tensor OrientedBoundingEllipsoid::GetMinBound() const { + return GetEllipsoidPoints().Min({0}); +} + +core::Tensor OrientedBoundingEllipsoid::GetMaxBound() const { + return GetEllipsoidPoints().Max({0}); +} + +double OrientedBoundingEllipsoid::Volume() const { + core::Tensor radii_prod = GetRadii().Prod({0}).To(core::Float64); + return 4.0 * M_PI * radii_prod.Item() / 3.0; +} + +core::Tensor OrientedBoundingEllipsoid::GetEllipsoidPoints() const { + // Create six critical points along the principal axes + // Extract radii values + core::Tensor radii_vals = GetRadii(); + double r0 = radii_vals.GetItem({core::TensorKey::Index(0)}).Item(); + double r1 = radii_vals.GetItem({core::TensorKey::Index(1)}).Item(); + double r2 = radii_vals.GetItem({core::TensorKey::Index(2)}).Item(); + + core::Tensor x_axis = GetRotation().Matmul(core::Tensor( + std::vector{r0, 0.0, 0.0}, {3}, GetDtype(), GetDevice())); + core::Tensor y_axis = GetRotation().Matmul(core::Tensor( + std::vector{0.0, r1, 0.0}, {3}, GetDtype(), GetDevice())); + core::Tensor z_axis = GetRotation().Matmul(core::Tensor( + std::vector{0.0, 0.0, r2}, {3}, GetDtype(), GetDevice())); + + // Create the six critical points + std::vector points; + points.push_back(GetCenter() + x_axis); + points.push_back(GetCenter() - x_axis); + points.push_back(GetCenter() + y_axis); + points.push_back(GetCenter() - y_axis); + points.push_back(GetCenter() + z_axis); + points.push_back(GetCenter() - z_axis); + + return core::Concatenate(points).Reshape({6, 3}); +} + +OrientedBoundingEllipsoid &OrientedBoundingEllipsoid::Translate( + const core::Tensor &translation, bool relative) { + core::AssertTensorDevice(translation, GetDevice()); + core::AssertTensorShape(translation, {3}); + core::AssertTensorDtypes(translation, {core::Float32, core::Float64}); + + const core::Tensor translation_d = translation.To(GetDtype()); + if (relative) { + center_ += translation_d; + } else { + center_ = translation_d; + } + return *this; +} + +OrientedBoundingEllipsoid &OrientedBoundingEllipsoid::Rotate( + const core::Tensor &rotation, + const std::optional ¢er) { + core::AssertTensorDevice(rotation, GetDevice()); + core::AssertTensorShape(rotation, {3, 3}); + core::AssertTensorDtypes(rotation, {core::Float32, core::Float64}); + + if (!rotation.T().AllClose(rotation.Inverse(), 1e-5, 1e-5)) { + utility::LogWarning( + "Invalid rotation matrix. Please make sure the rotation " + "matrix is orthogonal."); + return *this; + } + + const core::Tensor rotation_d = rotation.To(GetDtype()); + rotation_ = rotation_d.Matmul(rotation_); + if (center.has_value()) { + core::AssertTensorDevice(center.value(), GetDevice()); + core::AssertTensorShape(center.value(), {3}); + core::AssertTensorDtypes(center.value(), + {core::Float32, core::Float64}); + + core::Tensor center_d = center.value().To(GetDtype()); + center_ = rotation_d.Matmul(center_ - center_d).Flatten() + center_d; + } + + return *this; +} + +OrientedBoundingEllipsoid &OrientedBoundingEllipsoid::Scale( + const double scale, const std::optional ¢er) { + radii_ *= scale; + if (center.has_value()) { + core::Tensor center_d = center.value(); + core::AssertTensorDevice(center_d, GetDevice()); + core::AssertTensorShape(center_d, {3}); + core::AssertTensorDtypes(center_d, {core::Float32, core::Float64}); + + center_d = center_d.To(GetDtype()); + center_ = scale * (center_ - center_d) + center_d; + } + return *this; +} + +core::Tensor OrientedBoundingEllipsoid::GetPointIndicesWithinBoundingEllipsoid( + const core::Tensor &points) const { + core::AssertTensorDevice(points, GetDevice()); + core::AssertTensorShape(points, {std::nullopt, 3}); + core::AssertTensorDtypes(points, {core::Float32, core::Float64}); + + // Transform points to ellipsoid's local frame + core::Tensor local_points = (points - GetCenter()).Matmul(GetRotation()); + + // Normalize by radii + core::Tensor normalized = local_points / GetRadii(); + + // Check if norm squared is <= 1 + core::Tensor norm_squared = (normalized * normalized).Sum({1}); + core::Tensor mask = norm_squared.Le(1.0); + + return mask.NonZero().Flatten(); +} + +std::string OrientedBoundingEllipsoid::ToString() const { + return fmt::format("OrientedBoundingEllipsoid[{}, {}]", + GetDtype().ToString(), GetDevice().ToString()); +} + +open3d::geometry::OrientedBoundingEllipsoid +OrientedBoundingEllipsoid::ToLegacy() const { + open3d::geometry::OrientedBoundingEllipsoid legacy_ellipsoid; + + legacy_ellipsoid.center_ = + core::eigen_converter::TensorToEigenVector3dVector( + GetCenter().Reshape({1, 3}))[0]; + legacy_ellipsoid.radii_ = + core::eigen_converter::TensorToEigenVector3dVector( + GetRadii().Reshape({1, 3}))[0]; + legacy_ellipsoid.R_ = + core::eigen_converter::TensorToEigenMatrixXd(GetRotation()); + legacy_ellipsoid.color_ = + core::eigen_converter::TensorToEigenVector3dVector( + GetColor().Reshape({1, 3}))[0]; + return legacy_ellipsoid; +} + +AxisAlignedBoundingBox OrientedBoundingEllipsoid::GetAxisAlignedBoundingBox() + const { + return AxisAlignedBoundingBox::CreateFromPoints(GetEllipsoidPoints()); +} + +OrientedBoundingEllipsoid OrientedBoundingEllipsoid::FromLegacy( + const open3d::geometry::OrientedBoundingEllipsoid &ellipsoid, + const core::Dtype &dtype, + const core::Device &device) { + if (dtype != core::Float32 && dtype != core::Float64) { + utility::LogError( + "Got data-type {}, but the supported data-type of the bounding " + "ellipsoid are Float32 and Float64.", + dtype.ToString()); + } + + OrientedBoundingEllipsoid t_ellipsoid( + core::eigen_converter::EigenMatrixToTensor(ellipsoid.center_) + .Flatten() + .To(device, dtype), + core::eigen_converter::EigenMatrixToTensor(ellipsoid.R_) + .To(device, dtype), + core::eigen_converter::EigenMatrixToTensor(ellipsoid.radii_) + .Flatten() + .To(device, dtype)); + + t_ellipsoid.SetColor( + core::eigen_converter::EigenMatrixToTensor(ellipsoid.color_) + .Flatten() + .To(device, dtype)); + return t_ellipsoid; +} + +OrientedBoundingEllipsoid OrientedBoundingEllipsoid::CreateFromPoints( + const core::Tensor &points, bool robust) { + core::AssertTensorShape(points, {std::nullopt, 3}); + core::AssertTensorDtypes(points, {core::Float32, core::Float64}); + if (points.GetShape(0) < 4) { + utility::LogError("Input point set has less than 4 points."); + } + return kernel::minimum_obe::ComputeMinimumOBEKhachiyan(points, robust); +} + } // namespace geometry } // namespace t } // namespace open3d diff --git a/cpp/open3d/t/geometry/BoundingVolume.h b/cpp/open3d/t/geometry/BoundingVolume.h index e033dcdb4fa..28249a88182 100644 --- a/cpp/open3d/t/geometry/BoundingVolume.h +++ b/cpp/open3d/t/geometry/BoundingVolume.h @@ -482,6 +482,232 @@ class OrientedBoundingBox : public Geometry, public DrawableGeometry { core::Tensor color_; }; +/// \class OrientedBoundingEllipsoid +/// \brief A bounding ellipsoid oriented along an arbitrary frame of reference. +/// +/// - (center, rotation, radii): The oriented bounding ellipsoid is defined by +/// its center position, rotation matrix and radii along the three principal +/// axes. +/// - Usage +/// - OrientedBoundingEllipsoid::GetCenter() +/// - OrientedBoundingEllipsoid::SetCenter(const core::Tensor ¢er) +/// - OrientedBoundingEllipsoid::GetRotation() +/// - OrientedBoundingEllipsoid::SetRotation(const core::Tensor +/// &rotation) +/// - OrientedBoundingEllipsoid::GetRadii() +/// - OrientedBoundingEllipsoid::SetRadii(const core::Tensor &radii) +/// - Value tensor of center and radii must have shape {3,}. +/// - Value tensor of rotation must have shape {3, 3}. +/// - Value tensor must have the same data type and device. +/// - Value tensor can only be float32 (default) or float64. +/// - The device of the tensor determines the device of the ellipsoid. +/// +/// - color: Color of the bounding ellipsoid. +/// - Usage +/// - OrientedBoundingEllipsoid::GetColor() +/// - OrientedBoundingEllipsoid::SetColor(const core::Tensor &color) +/// - Value tensor must have shape {3,}. +/// - Value tensor can only be float32 (default) or float64. +/// - Value tensor can only be range [0.0, 1.0]. +class OrientedBoundingEllipsoid : public Geometry, public DrawableGeometry { +public: + /// \brief Construct an empty OrientedBoundingEllipsoid on the provided + /// device. + OrientedBoundingEllipsoid( + const core::Device &device = core::Device("CPU:0")); + + /// \brief Construct an OrientedBoundingEllipsoid from center, rotation and + /// radii. + /// + /// The OrientedBoundingEllipsoid will be created on the device of the + /// given tensors, which must be on the same device and have the same data + /// type. + /// \param center Center of the bounding ellipsoid. Tensor of shape {3,}, + /// and type float32 or float64. + /// \param rotation Rotation matrix of the bounding ellipsoid. Tensor of + /// shape {3, 3}, and type float32 or float64. + /// \param radii Radii of the bounding ellipsoid along the three principal + /// axes. Tensor of shape {3,}, and type float32 or float64. + OrientedBoundingEllipsoid(const core::Tensor ¢er, + const core::Tensor &rotation, + const core::Tensor &radii); + + virtual ~OrientedBoundingEllipsoid() override {} + + /// \brief Returns the device attribute of this OrientedBoundingEllipsoid. + core::Device GetDevice() const override { return device_; } + + /// \brief Returns the data type attribute of this + /// OrientedBoundingEllipsoid. + core::Dtype GetDtype() const { return dtype_; } + + /// Transfer the OrientedBoundingEllipsoid to a specified device. + /// \param device The targeted device to convert to. + /// \param copy If true, a new OrientedBoundingEllipsoid is always created; + /// if false, the copy is avoided when the original + /// OrientedBoundingEllipsoid is already on the targeted device. + OrientedBoundingEllipsoid To(const core::Device &device, + bool copy = false) const; + + /// Returns copy of the OrientedBoundingEllipsoid on the same device. + OrientedBoundingEllipsoid Clone() const { + return To(GetDevice(), /*copy=*/true); + } + + OrientedBoundingEllipsoid &Clear() override; + + bool IsEmpty() const override { return Volume() == 0; } + + /// \brief Set the center of the ellipsoid. + /// If the data type of the given tensor differs from the data type of the + /// ellipsoid, an exception will be thrown. + /// + /// \param center Tensor with {3,} shape, and type float32 or float64. + void SetCenter(const core::Tensor ¢er); + + /// \brief Set the rotation matrix of the ellipsoid. + /// If the data type of the given tensor differs from the data type of the + /// ellipsoid, an exception will be thrown. + /// + /// \param rotation Tensor with {3, 3} shape, and type float32 or float64. + void SetRotation(const core::Tensor &rotation); + + /// \brief Set the radii of the ellipsoid. + /// If the data type of the given tensor differs from the data type of the + /// ellipsoid, an exception will be thrown. + /// + /// \param radii Tensor with {3,} shape, and type float32 or float64. + void SetRadii(const core::Tensor &radii); + + /// \brief Set the color of the ellipsoid. + /// + /// \param color Tensor with {3,} shape, and type float32 or float64, + /// with values in range [0.0, 1.0]. + void SetColor(const core::Tensor &color); + +public: + core::Tensor GetMinBound() const; + + core::Tensor GetMaxBound() const; + + core::Tensor GetColor() const { return color_; } + + core::Tensor GetCenter() const { return center_; } + + core::Tensor GetRotation() const { return rotation_; } + + core::Tensor GetRadii() const { return radii_; } + + /// \brief Translate the oriented ellipsoid by the given translation. + /// If relative is true, the translation is added to the center of the + /// ellipsoid. If false, the center will be assigned to the translation. + /// + /// \param translation Translation tensor of shape {3,}, type float32 or + /// float64, device same as the ellipsoid. + /// \param relative Whether to perform relative translation. + OrientedBoundingEllipsoid &Translate(const core::Tensor &translation, + bool relative = true); + + /// \brief Rotate the oriented ellipsoid by the given rotation matrix. If + /// the rotation matrix is not orthogonal, the rotation will not be applied. + /// The rotation center will be the ellipsoid center if it is not specified. + /// + /// \param rotation Rotation matrix of shape {3, 3}, type float32 or + /// float64, device same as the ellipsoid. + /// \param center Center of the rotation, default is null, which means use + /// center of the ellipsoid as rotation center. + OrientedBoundingEllipsoid &Rotate( + const core::Tensor &rotation, + const std::optional ¢er = std::nullopt); + + /// \brief Scale the oriented ellipsoid. + /// The scaling center will be the ellipsoid center if it is not specified. + /// + /// \param scale The scale parameter. + /// \param center Center used for the scaling operation. Tensor of shape + /// {3,}, type float32 or float64, device same as the ellipsoid. + OrientedBoundingEllipsoid &Scale( + double scale, + const std::optional ¢er = std::nullopt); + + /// Returns the volume of the bounding ellipsoid. + double Volume() const; + + /// \brief Returns the six critical points of the bounding ellipsoid. + /// + /// The Return tensor has shape {6, 3} and data type same as the ellipsoid. + /// + /// \verbatim + /// ------- x + /// /| + /// / | + /// / | z + /// y + /// 2 + /// .--|---. + /// .--' | '--. + /// .--' | '--. + /// .' | 4 '. + /// / | / \ + /// / | / \ + /// 0 |------------------|-------------------| 1 + /// \ / | / + /// \ / | / + /// '. 5 | .' + /// '--. | .--' + /// '--. | .--' + /// '--|---' + /// 3 + /// \endverbatim + core::Tensor GetEllipsoidPoints() const; + + /// \brief Indices to points that are within the bounding ellipsoid. + /// + /// \param points Tensor with {N, 3} shape, and type float32 or float64. + core::Tensor GetPointIndicesWithinBoundingEllipsoid( + const core::Tensor &points) const; + + /// Text description. + std::string ToString() const; + + /// Convert to a legacy Open3D oriented bounding ellipsoid. + open3d::geometry::OrientedBoundingEllipsoid ToLegacy() const; + + /// Convert to an axis-aligned box. + AxisAlignedBoundingBox GetAxisAlignedBoundingBox() const; + + /// Create an OrientedBoundingEllipsoid from a legacy Open3D oriented + /// bounding ellipsoid. + /// + /// \param ellipsoid Legacy OrientedBoundingEllipsoid. + /// \param dtype The data type of the ellipsoid for center, radii and color. + /// The default is float32. + /// \param device The device of the ellipsoid. The default is CPU:0. + static OrientedBoundingEllipsoid FromLegacy( + const open3d::geometry::OrientedBoundingEllipsoid &ellipsoid, + const core::Dtype &dtype = core::Float32, + const core::Device &device = core::Device("CPU:0")); + + /// Creates the oriented bounding ellipsoid with the smallest volume using + /// Khachiyan's algorithm. + /// \param points A list of points with data type of float32 or float64 (N x + /// 3 tensor, where N must be larger than 3). + /// \param robust If set to true uses a more robust method which works in + /// degenerate cases but introduces noise to the points coordinates. + /// \return OrientedBoundingEllipsoid with same data type and device as + /// input points. + static OrientedBoundingEllipsoid CreateFromPoints( + const core::Tensor &points, bool robust = false); + +protected: + core::Device device_ = core::Device("CPU:0"); + core::Dtype dtype_ = core::Float32; + core::Tensor center_; + core::Tensor rotation_; + core::Tensor radii_; + core::Tensor color_; +}; + } // namespace geometry } // namespace t -} // namespace open3d +} // namespace open3d \ No newline at end of file diff --git a/cpp/open3d/t/geometry/Geometry.h b/cpp/open3d/t/geometry/Geometry.h index e388e509bcb..c40e1f408c5 100644 --- a/cpp/open3d/t/geometry/Geometry.h +++ b/cpp/open3d/t/geometry/Geometry.h @@ -52,6 +52,8 @@ class Geometry : public core::IsDevice { OrientedBoundingBox = 11, /// AxisAlignedBoundingBox AxisAlignedBoundingBox = 12, + /// OrientedBoundingEllipsoid + OrientedBoundingEllipsoid = 13, }; public: @@ -121,6 +123,10 @@ enum class MethodOBBCreate { MINIMAL_JYLANKI ///< Minimal OBB by Jylanki }; +enum class MethodOBELCreate { + MINIMAL_APPROX, ///< Minimal OBEL approximation +}; + } // namespace geometry } // namespace t } // namespace open3d \ No newline at end of file diff --git a/cpp/open3d/t/geometry/LineSet.cpp b/cpp/open3d/t/geometry/LineSet.cpp index 95d26c27275..3cf34ee7ca7 100644 --- a/cpp/open3d/t/geometry/LineSet.cpp +++ b/cpp/open3d/t/geometry/LineSet.cpp @@ -212,6 +212,10 @@ OrientedBoundingBox LineSet::GetOrientedBoundingBox() const { return OrientedBoundingBox::CreateFromPoints(GetPointPositions()); } +OrientedBoundingEllipsoid LineSet::GetOrientedBoundingEllipsoid() const { + return OrientedBoundingEllipsoid::CreateFromPoints(GetPointPositions()); +} + LineSet &LineSet::PaintUniformColor(const core::Tensor &color) { core::AssertTensorShape(color, {3}); core::Tensor clipped_color = color.To(GetDevice()); diff --git a/cpp/open3d/t/geometry/LineSet.h b/cpp/open3d/t/geometry/LineSet.h index e65e1855485..b2c1b216c37 100644 --- a/cpp/open3d/t/geometry/LineSet.h +++ b/cpp/open3d/t/geometry/LineSet.h @@ -368,6 +368,9 @@ class LineSet : public Geometry, public DrawableGeometry { /// Create an oriented bounding box from point attribute "positions". OrientedBoundingBox GetOrientedBoundingBox() const; + /// Create an oriented bounding ellipsoid from point attribute "positions". + OrientedBoundingEllipsoid GetOrientedBoundingEllipsoid() const; + /// Sweeps the line set rotationally about an axis. /// \param angle The rotation angle in degree. /// \param axis The rotation axis. diff --git a/cpp/open3d/t/geometry/TriangleMesh.h b/cpp/open3d/t/geometry/TriangleMesh.h index 9a5ade04f3e..2a71ad882bb 100644 --- a/cpp/open3d/t/geometry/TriangleMesh.h +++ b/cpp/open3d/t/geometry/TriangleMesh.h @@ -402,6 +402,26 @@ class TriangleMesh : public Geometry, public DrawableGeometry { core::Dtype int_dtype = core::Int64, const core::Device &device = core::Device("CPU:0")); + /// Create a ellipsoid triangle mesh. The ellipsoid will be centered + /// at (0, 0, 0). + /// \param radius_x defines first radii of the ellipsoid. + /// \param radius_y defines second radii of the ellipsoid. + /// \param radius_z defines third radii of the ellipsoid. + /// \param resolution defines the resolution of the ellipsoid. + /// \param float_dtype Float32 or Float64, used to store floating point + /// values, e.g. vertices, normals, colors. + /// \param int_dtype Int32 or Int64, used to store index values, e.g. + /// triangles. + /// \param device The device where the resulting TriangleMesh resides in. + static TriangleMesh CreateEllipsoid( + double radius_x = 1.0, + double radius_y = 1.0, + double radius_z = 1.0, + int resolution = 20, + core::Dtype float_dtype = core::Float32, + core::Dtype int_dtype = core::Int64, + const core::Device &device = core::Device("CPU:0")); + /// Create a tetrahedron triangle mesh. The centroid of the mesh will be /// placed at (0, 0, 0) and the vertices have a distance of radius to the /// center. diff --git a/cpp/open3d/t/geometry/TriangleMeshFactory.cpp b/cpp/open3d/t/geometry/TriangleMeshFactory.cpp index 2873585bdbe..e85a4c7198d 100644 --- a/cpp/open3d/t/geometry/TriangleMeshFactory.cpp +++ b/cpp/open3d/t/geometry/TriangleMeshFactory.cpp @@ -96,6 +96,22 @@ TriangleMesh TriangleMesh::CreateSphere(double radius, return mesh; } +TriangleMesh TriangleMesh::CreateEllipsoid(double radius_x, + double radius_y, + double radius_z, + int resolution, + core::Dtype float_dtype, + core::Dtype int_dtype, + const core::Device &device) { + std::shared_ptr legacy_mesh = + open3d::geometry::TriangleMesh::CreateEllipsoid( + radius_x, radius_y, radius_z, resolution); + + TriangleMesh mesh = TriangleMesh::FromLegacy(*legacy_mesh, float_dtype, + int_dtype, device); + return mesh; +} + TriangleMesh TriangleMesh::CreateTetrahedron(double radius, core::Dtype float_dtype, core::Dtype int_dtype, diff --git a/cpp/open3d/t/geometry/kernel/CMakeLists.txt b/cpp/open3d/t/geometry/kernel/CMakeLists.txt index 223c4d4c7a8..7c520100a66 100644 --- a/cpp/open3d/t/geometry/kernel/CMakeLists.txt +++ b/cpp/open3d/t/geometry/kernel/CMakeLists.txt @@ -10,6 +10,7 @@ target_sources(tgeometry_kernel PRIVATE PointCloudCPU.cpp Metrics.cpp MinimumOBB.cpp + MinimumOBE.cpp TriangleMesh.cpp TriangleMeshCPU.cpp Transform.cpp diff --git a/cpp/open3d/t/geometry/kernel/MinimumOBE.cpp b/cpp/open3d/t/geometry/kernel/MinimumOBE.cpp new file mode 100644 index 00000000000..de9f1986da1 --- /dev/null +++ b/cpp/open3d/t/geometry/kernel/MinimumOBE.cpp @@ -0,0 +1,289 @@ +// ---------------------------------------------------------------------------- +// - Open3D: www.open3d.org - +// ---------------------------------------------------------------------------- +// Copyright (c) 2018-2024 www.open3d.org +// SPDX-License-Identifier: MIT +// ---------------------------------------------------------------------------- + +#include "open3d/t/geometry/kernel/MinimumOBE.h" + +#include + +#include "open3d/core/EigenConverter.h" +#include "open3d/core/TensorCheck.h" +#include "open3d/t/geometry/BoundingVolume.h" +#include "open3d/t/geometry/PointCloud.h" + +namespace open3d { +namespace t { +namespace geometry { +namespace kernel { +namespace minimum_obe { + +namespace { + +// Helper struct using Eigen datatypes on the stack for OBE computation +struct EigenOBE { + EigenOBE() + : R_(Eigen::Matrix3d::Identity()), + radii_(Eigen::Vector3d::Zero()), + center_(Eigen::Vector3d::Zero()) {} + EigenOBE(const OrientedBoundingEllipsoid& obe) + : R_(core::eigen_converter::TensorToEigenMatrixXd(obe.GetRotation())), + radii_(core::eigen_converter::TensorToEigenMatrixXd( + obe.GetRadii().Reshape({3, 1}))), + center_(core::eigen_converter::TensorToEigenMatrixXd( + obe.GetCenter().Reshape({3, 1}))) {} + double Volume() const { + return 4.0 * M_PI * radii_(0) * radii_(1) * radii_(2) / 3.0; + } + operator OrientedBoundingEllipsoid() const { + OrientedBoundingEllipsoid obe; + obe.SetRotation(core::eigen_converter::EigenMatrixToTensor(R_)); + obe.SetRadii(core::eigen_converter::EigenMatrixToTensor(radii_).Reshape( + {3})); + obe.SetCenter( + core::eigen_converter::EigenMatrixToTensor(center_).Reshape( + {3})); + return obe; + } + Eigen::Matrix3d R_; + Eigen::Vector3d radii_; + Eigen::Vector3d center_; +}; + +// Map the ellipsoid orientation to the closest identity matrix +// This ensures a canonical representation +void MapOBEToClosestIdentity(EigenOBE& obe) { + Eigen::Matrix3d& R = obe.R_; + Eigen::Vector3d& radii = obe.radii_; + Eigen::Vector3d col[3] = {R.col(0), R.col(1), R.col(2)}; + double best_score = -1e9; + Eigen::Matrix3d best_R; + Eigen::Vector3d best_radii; + + // Hard-coded permutations of indices [0,1,2] + static const std::array, 6> permutations = { + {{{0, 1, 2}}, + {{0, 2, 1}}, + {{1, 0, 2}}, + {{1, 2, 0}}, + {{2, 0, 1}}, + {{2, 1, 0}}}}; + + // Evaluate all 6 permutations × 8 sign flips = 48 candidates + for (const auto& p : permutations) { + for (int sign_bits = 0; sign_bits < 8; ++sign_bits) { + // Derive the sign of each axis from bits (0 => -1, 1 => +1) + const int s0 = (sign_bits & 1) ? 1 : -1; + const int s1 = (sign_bits & 2) ? 1 : -1; + const int s2 = (sign_bits & 4) ? 1 : -1; + + // Construct candidate columns + Eigen::Vector3d c0 = s0 * col[p[0]]; + Eigen::Vector3d c1 = s1 * col[p[1]]; + Eigen::Vector3d c2 = s2 * col[p[2]]; + + // Score: how close are we to the identity? + // Since e_x = (1,0,0), e_y = (0,1,0), e_z = (0,0,1), + // we can skip dot products & do c0(0)+c1(1)+c2(2). + double score = c0(0) + c1(1) + c2(2); + + // If this orientation is better, update the best. + if (score > best_score) { + best_score = score; + best_R.col(0) = c0; + best_R.col(1) = c1; + best_R.col(2) = c2; + + // Re-permute radii: if the axis p[0] in old frame + // now goes to new X, etc. + best_radii(0) = radii(p[0]); + best_radii(1) = radii(p[1]); + best_radii(2) = radii(p[2]); + } + } + } + + // Update the OBE with the best orientation found + obe.R_ = best_R; + obe.radii_ = best_radii; +} + +// Khachiyan's algorithm for computing the minimum volume enclosing ellipsoid +// Returns the final epsilon (convergence measure) +double KhachiyanAlgorithm(const Eigen::MatrixXd& A, + double eps, + int maxiter, + int numVertices, + Eigen::MatrixXd& Q, + Eigen::VectorXd& c) { + // Initialize uniform weights: p_i = 1/N for all i + Eigen::VectorXd p = Eigen::VectorXd::Constant( + numVertices, 1.0 / static_cast(numVertices)); + + // Lift matrix A to Ap by adding a bottom row of ones + Eigen::MatrixXd Ap(4, numVertices); + Ap.topRows(3) = A; + Ap.row(3).setOnes(); + + double currentEps = 2.0 * eps; // Start with a large difference + int iter = 0; + + // Main iterative loop + while (iter < maxiter && currentEps > eps) { + // Compute Λ_p = Ap * diag(p) * Ap^T in a more efficient way + // ApP = Ap * diag(p) => shape: (4) x N + // Then Lambda_p = ApP * Ap^T => shape: (4) x (4) + Eigen::MatrixXd ApP = Ap * p.asDiagonal(); // (4) x N + Eigen::Matrix4d LambdaP = ApP * Ap.transpose(); // (4) x (4) + + // Compute inverse of Lambda_p via an LDLT factorization + // (faster and more numerically stable than .inverse()) + Eigen::LDLT ldltOfLambdaP(LambdaP); + if (ldltOfLambdaP.info() != Eigen::Success) { + throw std::runtime_error( + "LDLT decomposition failed. Matrix may be singular."); + } + + // M = Ap^T * (Lambda_p^{-1} * Ap) + // We'll do this in steps: + // 1) X = Lambda_p^{-1} * Ap + // 2) M = Ap^T * X + Eigen::MatrixXd X = ldltOfLambdaP.solve(Ap); // (4) x N + Eigen::MatrixXd M = Ap.transpose() * X; // NxN + + // Find max diagonal element and index + Eigen::Index maxIndex; + double maxVal = M.diagonal().maxCoeff(&maxIndex); + + // Compute step size alpha (called step_size here) + // Formula: alpha = (maxVal - 4) / (4 * (maxVal - 1)) + const double step_size = (maxVal - 4) / (4 * (maxVal - 1.0)); + + // Update weights p + Eigen::VectorXd newP = (1.0 - step_size) * p; + newP(maxIndex) += step_size; + + // Compute the change for the stopping criterion + currentEps = (newP - p).norm(); + p.swap(newP); // Efficient swap instead of copy + + ++iter; + } + + // After convergence or reaching max iterations, + // compute Q and center c for the ellipsoid. + + // 1) PN = A * diag(p) * A^T + Eigen::MatrixXd AP = A * p.asDiagonal(); // 3 x N + Eigen::Matrix3d PN = AP * A.transpose(); // 3 x 3 + + // 2) M2 = A * p => a 3-dimensional vector + Eigen::Vector3d M2 = A * p; // 3 x 1 + + // 3) M3 = M2 * M2^T => 3 x 3 outer product + Eigen::Matrix3d M3 = M2 * M2.transpose(); // 3 x 3 + + // 4) Invert (PN - M3) via LDLT + Eigen::Matrix3d toInvert = PN - M3; // 3 x 3 + Eigen::LDLT ldltOfToInvert(toInvert); + if (ldltOfToInvert.info() != Eigen::Success) { + throw std::runtime_error("LDLT decomposition failed in final step."); + } + + // Q = (toInvert)^{-1} / 3 => shape matrix of the ellipsoid + // c = A * p => center of the ellipsoid + Q = ldltOfToInvert.solve(Eigen::Matrix3d::Identity()) / + static_cast(3); + c = M2; // Already computed as A*p + + return currentEps; +} + +} // namespace + +OrientedBoundingEllipsoid ComputeMinimumOBEKhachiyan( + const core::Tensor& points_, bool robust) { + // ------------------------------------------------------------ + // 0) Compute the convex hull of the input point cloud + // ------------------------------------------------------------ + core::AssertTensorShape(points_, {std::nullopt, 3}); + if (points_.GetShape(0) == 0) { + utility::LogError("Input point set is empty."); + return OrientedBoundingEllipsoid(); + } + if (points_.GetShape(0) < 4) { + utility::LogError("Input point set has less than 4 points."); + return OrientedBoundingEllipsoid(); + } + + // Copy to CPU here + PointCloud pcd(points_.To(core::Device())); + auto hull_mesh = pcd.ComputeConvexHull(robust); + if (hull_mesh.GetVertexPositions().NumElements() == 0) { + utility::LogError("Failed to compute convex hull."); + return OrientedBoundingEllipsoid(); + } + + // Get convex hull vertices + const std::vector& hull_v = + core::eigen_converter::TensorToEigenVector3dVector( + hull_mesh.GetVertexPositions()); + int num_vertices = static_cast(hull_v.size()); + + // Handle degenerate planar cases up front. + if (num_vertices < 4) { + utility::LogError("Convex hull is degenerate."); + return OrientedBoundingEllipsoid(); + } + + // Assemble matrix A with dimensions 3 x n_points, where each column is a + // point + Eigen::MatrixXd A(3, num_vertices); + for (int i = 0; i < num_vertices; ++i) { + A.col(i) = hull_v[i]; + } + + // Set parameters for Khachiyan's algorithm + double eps = 1e-6; + int maxiter = 1000; + + // Variables to store the resulting ellipsoid parameters + Eigen::MatrixXd Q; + Eigen::VectorXd c; + + // Call Khachiyan's algorithm to compute Q (shape matrix) and c (center) + KhachiyanAlgorithm(A, eps, maxiter, num_vertices, Q, c); + + // Use eigen-decomposition on Q to extract axes lengths and orientation + // For ellipsoid defined by (x-c)^T Q (x-c) <= 1, + // the eigenvectors of Q give orientation directions, + // and the axes lengths (radii) are 1/sqrt(eigenvalues). + Eigen::SelfAdjointEigenSolver eigenSolver(Q); + if (eigenSolver.info() != Eigen::Success) { + throw std::runtime_error("Eigen decomposition failed."); + } + Eigen::VectorXd eigenvalues = eigenSolver.eigenvalues(); + Eigen::MatrixXd eigenvectors = eigenSolver.eigenvectors(); + + // Compute radii = 1/sqrt(eigenvalues) + Eigen::Vector3d radii = (1.0 / eigenvalues.array().sqrt()).matrix(); + + // Construct the final oriented bounding ellipsoid + EigenOBE obe; + obe.center_ = c.head<3>(); // center vector of length 3 + obe.R_ = eigenvectors; // orientation matrix + obe.radii_ = radii; // ellipsoid radii + + // Check orientation and permute axes to closest identity + MapOBEToClosestIdentity(obe); + + return obe; +} + +} // namespace minimum_obe +} // namespace kernel +} // namespace geometry +} // namespace t +} // namespace open3d \ No newline at end of file diff --git a/cpp/open3d/t/geometry/kernel/MinimumOBE.h b/cpp/open3d/t/geometry/kernel/MinimumOBE.h new file mode 100644 index 00000000000..a97aeecc643 --- /dev/null +++ b/cpp/open3d/t/geometry/kernel/MinimumOBE.h @@ -0,0 +1,44 @@ +// ---------------------------------------------------------------------------- +// - Open3D: www.open3d.org - +// ---------------------------------------------------------------------------- +// Copyright (c) 2018-2024 www.open3d.org +// SPDX-License-Identifier: MIT +// ---------------------------------------------------------------------------- +#pragma once + +#include "open3d/t/geometry/BoundingVolume.h" +#include "open3d/t/geometry/TriangleMesh.h" + +namespace open3d { +namespace t { +namespace geometry { + +// Forward declaration +class OrientedBoundingEllipsoid; + +namespace kernel { +namespace minimum_obe { + +/// Creates the oriented bounding ellipsoid with the smallest volume using +/// Khachiyan's algorithm. This algorithm computes the minimum volume enclosing +/// ellipsoid (MVEE) around a set of points. The MVEE is unique and can be +/// computed to arbitrary precision using an iterative algorithm. +/// +/// The algorithm works by: +/// 1. Computing the convex hull of the input points +/// 2. Running Khachiyan's algorithm to find the optimal ellipsoid +/// 3. Extracting the ellipsoid parameters (center, orientation, radii) +/// +/// \param points A list of points with data type of float32 or float64 (N x +/// 3 tensor, where N must be larger than 3). +/// \param robust If set to true uses a more robust method which works +/// in degenerate cases but introduces noise to the points +/// coordinates. +OrientedBoundingEllipsoid ComputeMinimumOBEKhachiyan(const core::Tensor &points, + bool robust); + +} // namespace minimum_obe +} // namespace kernel +} // namespace geometry +} // namespace t +} // namespace open3d \ No newline at end of file diff --git a/cpp/open3d/utility/CMakeLists.txt b/cpp/open3d/utility/CMakeLists.txt index d7211a0cefc..7ccb5ca05c6 100644 --- a/cpp/open3d/utility/CMakeLists.txt +++ b/cpp/open3d/utility/CMakeLists.txt @@ -19,6 +19,11 @@ target_sources(utility PRIVATE Timer.cpp ) +target_compile_definitions(utility PRIVATE + _FILE_OFFSET_BITS=64 + _LARGEFILE_SOURCE=1 +) + if (BUILD_ISPC_MODULE) target_sources(utility PRIVATE ISAInfo.ispc diff --git a/cpp/open3d/visualization/rendering/filament/FilamentGeometryBuffersBuilder.cpp b/cpp/open3d/visualization/rendering/filament/FilamentGeometryBuffersBuilder.cpp index a11c8d96577..871387a3ce0 100644 --- a/cpp/open3d/visualization/rendering/filament/FilamentGeometryBuffersBuilder.cpp +++ b/cpp/open3d/visualization/rendering/filament/FilamentGeometryBuffersBuilder.cpp @@ -223,6 +223,14 @@ std::unique_ptr GeometryBuffersBuilder::GetBuilder( lines->PaintUniformColor(obb.color_); return std::make_unique(lines); } + case GT::OrientedBoundingEllipsoid: { + auto obel = static_cast( + geometry); + auto lines = geometry::LineSet::CreateFromOrientedBoundingEllipsoid( + obel); + lines->PaintUniformColor(obel.color_); + return std::make_unique(lines); + } case GT::AxisAlignedBoundingBox: { auto aabb = static_cast( geometry); diff --git a/cpp/open3d/visualization/shader/GeometryRenderer.cpp b/cpp/open3d/visualization/shader/GeometryRenderer.cpp index fb24a822ebf..a3992f3f71f 100644 --- a/cpp/open3d/visualization/shader/GeometryRenderer.cpp +++ b/cpp/open3d/visualization/shader/GeometryRenderer.cpp @@ -183,6 +183,28 @@ bool TetraMeshRenderer::UpdateGeometry() { return true; } +bool OrientedBoundingEllipsoidRenderer::Render(const RenderOption &option, + const ViewControl &view) { + if (!is_visible_ || geometry_ptr_->IsEmpty()) return true; + return simple_oriented_bounding_ellipsoid_shader_.Render(*geometry_ptr_, + option, view); +} + +bool OrientedBoundingEllipsoidRenderer::AddGeometry( + std::shared_ptr geometry_ptr) { + if (geometry_ptr->GetGeometryType() != + geometry::Geometry::GeometryType::OrientedBoundingEllipsoid) { + return false; + } + geometry_ptr_ = geometry_ptr; + return UpdateGeometry(); +} + +bool OrientedBoundingEllipsoidRenderer::UpdateGeometry() { + simple_oriented_bounding_ellipsoid_shader_.InvalidateGeometry(); + return true; +} + bool OrientedBoundingBoxRenderer::Render(const RenderOption &option, const ViewControl &view) { if (!is_visible_ || geometry_ptr_->IsEmpty()) return true; diff --git a/cpp/open3d/visualization/shader/GeometryRenderer.h b/cpp/open3d/visualization/shader/GeometryRenderer.h index 5952c6a3e48..ae063af575e 100644 --- a/cpp/open3d/visualization/shader/GeometryRenderer.h +++ b/cpp/open3d/visualization/shader/GeometryRenderer.h @@ -123,6 +123,20 @@ class TetraMeshRenderer : public GeometryRenderer { SimpleShaderForTetraMesh simple_tetramesh_shader_; }; +class OrientedBoundingEllipsoidRenderer : public GeometryRenderer { +public: + ~OrientedBoundingEllipsoidRenderer() override {} + +public: + bool Render(const RenderOption &option, const ViewControl &view) override; + bool AddGeometry( + std::shared_ptr geometry_ptr) override; + bool UpdateGeometry() override; + +protected: + SimpleShaderForOrientedBoundingEllipsoid + simple_oriented_bounding_ellipsoid_shader_; +}; class OrientedBoundingBoxRenderer : public GeometryRenderer { public: ~OrientedBoundingBoxRenderer() override {} diff --git a/cpp/open3d/visualization/shader/SimpleShader.cpp b/cpp/open3d/visualization/shader/SimpleShader.cpp index 44278e97474..863390b1400 100644 --- a/cpp/open3d/visualization/shader/SimpleShader.cpp +++ b/cpp/open3d/visualization/shader/SimpleShader.cpp @@ -308,6 +308,55 @@ bool SimpleShaderForTetraMesh::PrepareBinding( return true; } +bool SimpleShaderForOrientedBoundingEllipsoid::PrepareRendering( + const geometry::Geometry &geometry, + const RenderOption &option, + const ViewControl &view) { + if (geometry.GetGeometryType() != + geometry::Geometry::GeometryType::OrientedBoundingEllipsoid) { + PrintShaderWarning( + "Rendering type is not geometry::OrientedBoundingEllipsoid."); + return false; + } + glLineWidth(GLfloat(option.line_width_)); + glEnable(GL_DEPTH_TEST); + glDepthFunc(GLenum(option.GetGLDepthFunc())); + return true; +} + +bool SimpleShaderForOrientedBoundingEllipsoid::PrepareBinding( + const geometry::Geometry &geometry, + const RenderOption &option, + const ViewControl &view, + std::vector &points, + std::vector &colors) { + if (geometry.GetGeometryType() != + geometry::Geometry::GeometryType::OrientedBoundingEllipsoid) { + PrintShaderWarning( + "Rendering type is not geometry::OrientedBoundingEllipsoid."); + return false; + } + auto lineset = geometry::LineSet::CreateFromOrientedBoundingEllipsoid( + (const geometry::OrientedBoundingEllipsoid &)geometry); + points.resize(lineset->lines_.size() * 2); + colors.resize(lineset->lines_.size() * 2); + for (size_t i = 0; i < lineset->lines_.size(); i++) { + const auto point_pair = lineset->GetLineCoordinate(i); + points[i * 2] = point_pair.first.cast(); + points[i * 2 + 1] = point_pair.second.cast(); + Eigen::Vector3d color; + if (lineset->HasColors()) { + color = lineset->colors_[i]; + } else { + color = Eigen::Vector3d::Zero(); + } + colors[i * 2] = colors[i * 2 + 1] = color.cast(); + } + draw_arrays_mode_ = GL_LINES; + draw_arrays_size_ = GLsizei(points.size()); + return true; +} + bool SimpleShaderForOrientedBoundingBox::PrepareRendering( const geometry::Geometry &geometry, const RenderOption &option, diff --git a/cpp/open3d/visualization/shader/SimpleShader.h b/cpp/open3d/visualization/shader/SimpleShader.h index e5bd2cbaa23..98f10c03579 100644 --- a/cpp/open3d/visualization/shader/SimpleShader.h +++ b/cpp/open3d/visualization/shader/SimpleShader.h @@ -98,6 +98,21 @@ class SimpleShaderForTetraMesh : public SimpleShader { std::vector &colors) final; }; +class SimpleShaderForOrientedBoundingEllipsoid : public SimpleShader { +public: + SimpleShaderForOrientedBoundingEllipsoid() + : SimpleShader("SimpleShaderForOrientedBoundingEllipsoid") {} + +protected: + bool PrepareRendering(const geometry::Geometry &geometry, + const RenderOption &option, + const ViewControl &view) final; + bool PrepareBinding(const geometry::Geometry &geometry, + const RenderOption &option, + const ViewControl &view, + std::vector &points, + std::vector &colors) final; +}; class SimpleShaderForOrientedBoundingBox : public SimpleShader { public: SimpleShaderForOrientedBoundingBox() diff --git a/cpp/open3d/visualization/utility/PointCloudPicker.cpp b/cpp/open3d/visualization/utility/PointCloudPicker.cpp index 77c8f2f05a3..8e3d287a13f 100644 --- a/cpp/open3d/visualization/utility/PointCloudPicker.cpp +++ b/cpp/open3d/visualization/utility/PointCloudPicker.cpp @@ -79,6 +79,17 @@ geometry::OrientedBoundingBox PointCloudPicker::GetMinimalOrientedBoundingBox( } } +geometry::OrientedBoundingEllipsoid +PointCloudPicker::GetOrientedBoundingEllipsoid(bool robust) const { + if (pointcloud_ptr_) { + return geometry::OrientedBoundingEllipsoid::CreateFromPoints( + ((const geometry::PointCloud&)(*pointcloud_ptr_)).points_, + robust); + } else { + return geometry::OrientedBoundingEllipsoid(); + } +} + PointCloudPicker& PointCloudPicker::Transform( const Eigen::Matrix4d& /*transformation*/) { // Do nothing diff --git a/cpp/open3d/visualization/utility/PointCloudPicker.h b/cpp/open3d/visualization/utility/PointCloudPicker.h index 915e65b44e9..e57d18f62c6 100644 --- a/cpp/open3d/visualization/utility/PointCloudPicker.h +++ b/cpp/open3d/visualization/utility/PointCloudPicker.h @@ -60,6 +60,16 @@ class PointCloudPicker : public geometry::Geometry3D { geometry::OrientedBoundingBox GetMinimalOrientedBoundingBox( bool robust = false) const final; + /// If point cloud does not exist, creates an oriented bounding ellipsoid + /// form its default constructor. If point cloud exists, creates an + /// oriented bounding ellipsoid around it. + /// 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. + geometry::OrientedBoundingEllipsoid GetOrientedBoundingEllipsoid( + bool robust = false) const final; + PointCloudPicker& Transform(const Eigen::Matrix4d& transformation) override; PointCloudPicker& Translate(const Eigen::Vector3d& translation, bool relative = true) override; diff --git a/cpp/open3d/visualization/visualizer/Visualizer.cpp b/cpp/open3d/visualization/visualizer/Visualizer.cpp index 85c87fb02f4..b449e0de8b0 100644 --- a/cpp/open3d/visualization/visualizer/Visualizer.cpp +++ b/cpp/open3d/visualization/visualizer/Visualizer.cpp @@ -399,6 +399,13 @@ bool Visualizer::AddGeometry( if (!renderer_ptr->AddGeometry(geometry_ptr)) { return false; } + } else if (geometry_ptr->GetGeometryType() == + geometry::Geometry::GeometryType::OrientedBoundingEllipsoid) { + renderer_ptr = + std::make_shared(); + if (!renderer_ptr->AddGeometry(geometry_ptr)) { + return false; + } } else if (geometry_ptr->GetGeometryType() == geometry::Geometry::GeometryType::OrientedBoundingBox) { renderer_ptr = std::make_shared(); diff --git a/cpp/open3d/visualization/visualizer/VisualizerWithVertexSelection.cpp b/cpp/open3d/visualization/visualizer/VisualizerWithVertexSelection.cpp index d7c520da6a2..6a418b2c598 100644 --- a/cpp/open3d/visualization/visualizer/VisualizerWithVertexSelection.cpp +++ b/cpp/open3d/visualization/visualizer/VisualizerWithVertexSelection.cpp @@ -115,6 +115,7 @@ bool VisualizerWithVertexSelection::AddGeometry( case geometry::Geometry::GeometryType::Octree: case geometry::Geometry::GeometryType::OrientedBoundingBox: case geometry::Geometry::GeometryType::AxisAlignedBoundingBox: + case geometry::Geometry::GeometryType::OrientedBoundingEllipsoid: case geometry::Geometry::GeometryType::Unspecified: return false; } @@ -214,6 +215,7 @@ bool VisualizerWithVertexSelection::UpdateGeometry( case geometry::Geometry::GeometryType::Octree: case geometry::Geometry::GeometryType::OrientedBoundingBox: case geometry::Geometry::GeometryType::AxisAlignedBoundingBox: + case geometry::Geometry::GeometryType::OrientedBoundingEllipsoid: case geometry::Geometry::GeometryType::Unspecified: break; } @@ -786,6 +788,7 @@ VisualizerWithVertexSelection::GetGeometryPoints( case geometry::Geometry::GeometryType::Octree: case geometry::Geometry::GeometryType::OrientedBoundingBox: case geometry::Geometry::GeometryType::AxisAlignedBoundingBox: + case geometry::Geometry::GeometryType::OrientedBoundingEllipsoid: case geometry::Geometry::GeometryType::Unspecified: points = nullptr; break; diff --git a/cpp/pybind/CMakeLists.txt b/cpp/pybind/CMakeLists.txt index 4da6d51aebd..6ed843136dc 100644 --- a/cpp/pybind/CMakeLists.txt +++ b/cpp/pybind/CMakeLists.txt @@ -48,22 +48,38 @@ if (WIN32) target_link_options(pybind PUBLIC "/force:multiple") # TODO(Sameer): Only export PyInit_pybind, open3d_core_cuda_device_count elseif (APPLE) - file(GENERATE OUTPUT pybind.map CONTENT - [=[_PyInit_pybind + if (BUILD_CUDA_MODULE) + file(GENERATE OUTPUT pybind.map CONTENT + [=[_PyInit_pybind open3d_core_cuda_device_count - ]=]) + ]=]) + else() + file(GENERATE OUTPUT pybind.map CONTENT + [=[_PyInit_pybind + ]=]) + endif() target_link_options(pybind PRIVATE $<$: -Wl,-exported_symbols_list "${CMAKE_CURRENT_BINARY_DIR}/pybind.map" >) elseif (UNIX) # Linux - file(GENERATE OUTPUT pybind.map CONTENT - [=[{ + if (BUILD_CUDA_MODULE) + file(GENERATE OUTPUT pybind.map CONTENT + [=[{ global: PyInit_pybind; open3d_core_cuda_device_count; local: *; };]=]) + else() + file(GENERATE OUTPUT pybind.map CONTENT + [=[{ + global: + PyInit_pybind; + local: + *; +};]=]) + endif() target_link_options(pybind PRIVATE $<$: "-Wl,--version-script=${CMAKE_CURRENT_BINARY_DIR}/pybind.map" >) target_link_options(pybind PRIVATE "-flto=auto") @@ -274,4 +290,4 @@ add_custom_target(install-python-package COMMAND ${Python3_EXECUTABLE} setup.py install --single-version-externally-managed --root=/ WORKING_DIRECTORY ${PYTHON_PACKAGE_DST_DIR} DEPENDS python-package -) +) \ No newline at end of file diff --git a/cpp/pybind/geometry/boundingvolume.cpp b/cpp/pybind/geometry/boundingvolume.cpp index 6de062074e3..a9d2277f38c 100644 --- a/cpp/pybind/geometry/boundingvolume.cpp +++ b/cpp/pybind/geometry/boundingvolume.cpp @@ -30,8 +30,93 @@ void pybind_boundingvolume_declarations(py::module &m) { "geometries, The axis aligned bounding " "box uses the coordinate axes for " "bounding box generation."); + py::class_, + std::shared_ptr, Geometry3D> + oriented_bounding_ellipsoid( + m, "OrientedBoundingEllipsoid", + "Class that defines an oriented ellipsoid that can " + "be computed from 3D geometries."); } void pybind_boundingvolume_definitions(py::module &m) { + auto oriented_bounding_ellipsoid = static_cast, + std::shared_ptr, Geometry3D>>( + m.attr("OrientedBoundingEllipsoid")); + py::detail::bind_default_constructor( + oriented_bounding_ellipsoid); + py::detail::bind_copy_functions( + oriented_bounding_ellipsoid); + oriented_bounding_ellipsoid + .def(py::init(), + "Create OrientedBoundingEllipsoid from center, rotation R and " + "radii " + "in x, y and z " + "direction", + "center"_a, "R"_a, "radii"_a) + .def("__repr__", + [](const OrientedBoundingEllipsoid &ellipsoid) { + std::stringstream s; + auto c = ellipsoid.center_; + auto r = ellipsoid.radii_; + s << "OrientedBoundingEllipsoid: center: (" << c.x() + << ", " << c.y() << ", " << c.z() + << "), radii: " << r.x() << ", " << r.y() << ", " + << r.z() << ")"; + return s.str(); + }) + // .def_static("create_from_axis_aligned_bounding_box", + // &OrientedBoundingEllipsoid:: + // CreateFromAxisAlignedBoundingBox, + // "Returns an oriented bounding ellipsoid from the " + // "AxisAlignedBoundingBox.", + // "aabox"_a) + // .def_static( + // "create_from_oriented_bounding_box", + // &OrientedBoundingEllipsoid::CreateFromOrientedBoundingBox, + // "Returns an oriented bounding ellipsoid from the " + // "OrientedBoundingBox.", + // "obb"_a) + .def_static("create_from_points", + &OrientedBoundingEllipsoid::CreateFromPoints, + "points"_a, "robust"_a = false, + R"doc( +Creates the oriented bounding ellipsoid that encloses the set of points. + +Computes the oriented bounding box based on the Khachiyan algorithm. +https://ecommons.cornell.edu/server/api/core/bitstreams/2b9e302e-e31c-45c9-b292-64a1c0d70bef/content +The returned bounding ellipsoid is the minimal volume bounding ellipsoid. + +Args: + points (open3d.utility.Vector3dVector): Input points. + robust (bool): If set to true uses a more robust method which works in + degenerate cases but introduces noise to the points coordinates. + +Returns: + open3d.geometry.OrientedBoundingBox: The oriented bounding box. The + bounding box is oriented such that the axes are ordered with respect to + the principal components. +)doc") + + .def("volume", &OrientedBoundingEllipsoid::Volume, + "Returns the volume of the bounding ellipsoid.") + .def_readwrite("center", &OrientedBoundingEllipsoid::center_, + "``float64`` array of shape ``(3, )``") + .def_readwrite("R", &OrientedBoundingEllipsoid::R_, + "``float64`` array of shape ``(3,3 )``") + .def_readwrite("radii", &OrientedBoundingEllipsoid::radii_, + "``float64`` array of shape ``(3, )``") + .def_readwrite("color", &OrientedBoundingEllipsoid::color_, + "``float64`` array of shape ``(3, )``"); + docstring::ClassMethodDocInject(m, "OrientedBoundingEllipsoid", "volume"); + // docstring::ClassMethodDocInject( + // m, "OrientedBoundingEllipsoid", + // "create_from_axis_aligned_bounding_box", + // {{"aabox", + // "AxisAlignedBoundingBox object from which + // OrientedBoundingEllipsoid is " "created."}}); + auto oriented_bounding_box = static_cast< py::class_, std::shared_ptr, Geometry3D>>( @@ -214,4 +299,4 @@ smallest) volume. } } // namespace geometry -} // namespace open3d +} // namespace open3d \ No newline at end of file diff --git a/cpp/pybind/geometry/geometry.cpp b/cpp/pybind/geometry/geometry.cpp index 7ed6a47507f..7bd4c67016d 100644 --- a/cpp/pybind/geometry/geometry.cpp +++ b/cpp/pybind/geometry/geometry.cpp @@ -129,6 +129,22 @@ at the end, return the box with the smallest volume Returns: open3d.geometry.OrientedBoundingBox: The oriented bounding box. The bounding box is oriented such that its volume is minimized. +)doc") + .def("get_oriented_bounding_ellipsoid", + &Geometry3D::GetOrientedBoundingEllipsoid, "robust"_a = false, + R"doc( +Returns the oriented bounding ellipsoid for the geometry. + +Computes the minimum volume enclosing ellipsoid using Khachiyan's algorithm. +The returned ellipsoid is the smallest volume ellipsoid that contains all points. + +Args: + robust (bool): If set to true uses a more robust method which works in + degenerate cases but introduces noise to the points coordinates. + +Returns: + open3d.geometry.OrientedBoundingEllipsoid: The oriented bounding ellipsoid. + The ellipsoid is defined by its center, radii, and rotation matrix. )doc") .def("transform", &Geometry3D::Transform, "Apply transformation (4x4 matrix) to the geometry " @@ -248,4 +264,4 @@ void pybind_geometry_definitions(py::module &m) { } } // namespace geometry -} // namespace open3d +} // namespace open3d \ No newline at end of file diff --git a/cpp/pybind/geometry/geometry_trampoline.h b/cpp/pybind/geometry/geometry_trampoline.h index 84afa97efbf..a7345cfff52 100644 --- a/cpp/pybind/geometry/geometry_trampoline.h +++ b/cpp/pybind/geometry/geometry_trampoline.h @@ -55,6 +55,11 @@ class PyGeometry3D : public PyGeometry { bool robust = false) const override { PYBIND11_OVERLOAD_PURE(OrientedBoundingBox, Geometry3DBase, robust); } + OrientedBoundingEllipsoid GetOrientedBoundingEllipsoid( + bool robust = false) const override { + PYBIND11_OVERLOAD_PURE(OrientedBoundingEllipsoid, Geometry3DBase, + robust); + } Geometry3DBase& Transform(const Eigen::Matrix4d& transformation) override { PYBIND11_OVERLOAD_PURE(Geometry3DBase&, Geometry3DBase, transformation); } diff --git a/cpp/pybind/geometry/lineset.cpp b/cpp/pybind/geometry/lineset.cpp index 637bdc0ea36..bc84d14df30 100644 --- a/cpp/pybind/geometry/lineset.cpp +++ b/cpp/pybind/geometry/lineset.cpp @@ -63,6 +63,11 @@ void pybind_lineset_definitions(py::module &m) { "Factory function to create a LineSet from an " "OrientedBoundingBox.", "box"_a) + .def_static("create_from_oriented_bounding_ellipsoid", + &LineSet::CreateFromOrientedBoundingEllipsoid, + "Factory function to create a LineSet from an " + "OrientedBoundingEllipsoid.", + "ellipsoid"_a) .def_static("create_from_axis_aligned_bounding_box", &LineSet::CreateFromAxisAlignedBoundingBox, "Factory function to create a LineSet from an " @@ -125,6 +130,9 @@ void pybind_lineset_definitions(py::module &m) { docstring::ClassMethodDocInject(m, "LineSet", "create_from_axis_aligned_bounding_box", {{"box", "The input bounding box."}}); + docstring::ClassMethodDocInject( + m, "LineSet", "create_from_oriented_bounding_ellipsoid", + {{"ellipsoid", "The input bounding ellipsoid."}}); docstring::ClassMethodDocInject(m, "LineSet", "create_from_triangle_mesh", {{"mesh", "The input triangle mesh."}}); docstring::ClassMethodDocInject(m, "LineSet", "create_from_tetra_mesh", diff --git a/cpp/pybind/geometry/trianglemesh.cpp b/cpp/pybind/geometry/trianglemesh.cpp index 818630e9542..7bc42be60b4 100644 --- a/cpp/pybind/geometry/trianglemesh.cpp +++ b/cpp/pybind/geometry/trianglemesh.cpp @@ -357,6 +357,12 @@ void pybind_trianglemesh_definitions(py::module &m) { "Factory function to create a solid oriented bounding box.", "obox"_a, "scale"_a = Eigen::Vector3d::Ones(), "create_uv_map"_a = false) + .def_static("create_from_oriented_bounding_ellipsoid", + &TriangleMesh::CreateFromOrientedBoundingEllipsoid, + "Factory function to create a solid oriented bounding " + "ellipsoid.", + "obel"_a, "scale"_a = Eigen::Vector3d::Ones(), + "resolution"_a = 20, "create_uv_map"_a = false) .def_static("create_box", &TriangleMesh::CreateBox, "Factory function to create a box. The left bottom " "corner on the " @@ -391,6 +397,12 @@ void pybind_trianglemesh_definitions(py::module &m) { "(0, 0, 0).", "radius"_a = 1.0, "resolution"_a = 20, "create_uv_map"_a = false) + .def_static( + "create_ellipsoid", &TriangleMesh::CreateEllipsoid, + "Factory function to create an ellipsoid mesh centered at " + "(0, 0, 0).", + "radius_x"_a = 1.0, "radius_y"_a = 1.0, "radius_z"_a = 1.0, + "resolution"_a = 20, "create_uv_map"_a = false) .def_static("create_cylinder", &TriangleMesh::CreateCylinder, "Factory function to create a cylinder mesh.", "radius"_a = 1.0, "height"_a = 2.0, "resolution"_a = 20, @@ -730,6 +742,18 @@ void pybind_trianglemesh_definitions(py::module &m) { "latitudes will be split into ```2 * resolution`` segments (i.e. " "there are ``2 * resolution`` longitude lines.)"}, {"create_uv_map", "Add default uv map to the mesh."}}); + docstring::ClassMethodDocInject( + m, "TriangleMesh", "create_ellipsoid", + {{"radius_x", "The first radius of the ellipsoid."}, + {"radius_y", "The second radius of the ellipsoid."}, + {"radius_z", "The third radius of the ellipsoid."}, + {"resolution", + "The resolution of the sphere. The longitues will be split into " + "``resolution`` segments (i.e. there are ``resolution + 1`` " + "latitude lines including the north and south pole). The " + "latitudes will be split into ```2 * resolution`` segments (i.e. " + "there are ``2 * resolution`` longitude lines.)"}, + {"create_uv_map", "Add default uv map to the mesh."}}); docstring::ClassMethodDocInject( m, "TriangleMesh", "create_cylinder", {{"radius", "The radius of the cylinder."}, diff --git a/cpp/pybind/t/geometry/trianglemesh.cpp b/cpp/pybind/t/geometry/trianglemesh.cpp index 20353b4848f..f469c791367 100644 --- a/cpp/pybind/t/geometry/trianglemesh.cpp +++ b/cpp/pybind/t/geometry/trianglemesh.cpp @@ -357,6 +357,13 @@ This example shows how to create a hemisphere from a sphere:: "float_dtype"_a = core::Float32, "int_dtype"_a = core::Int64, "device"_a = core::Device("CPU:0")) + .def_static("create_ellipsoid", &TriangleMesh::CreateEllipsoid, + "Create an ellipsoid mesh centered at (0, 0, 0).", + "radius_x"_a = 1.0, "radius_y"_a = 1.0, + "radius_z"_a = 1.0, "resolution"_a = 20, + "float_dtype"_a = core::Float32, + "int_dtype"_a = core::Int64, + "device"_a = core::Device("CPU:0")) .def_static("create_tetrahedron", &TriangleMesh::CreateTetrahedron, "Create a tetrahedron mesh centered at (0, 0, 0).", "radius"_a = 1.0, "float_dtype"_a = core::Float32, diff --git a/cpp/tests/t/geometry/TriangleMesh.cpp b/cpp/tests/t/geometry/TriangleMesh.cpp index cdcc3f40241..00901236f62 100644 --- a/cpp/tests/t/geometry/TriangleMesh.cpp +++ b/cpp/tests/t/geometry/TriangleMesh.cpp @@ -644,6 +644,47 @@ TEST_P(TriangleMeshPermuteDevices, CreateSphere) { triangle_indices_custom)); } +TEST_P(TriangleMeshPermuteDevices, CreateEllipsoid) { + core::Device device = GetParam(); + core::Dtype float_dtype_custom = core::Float64; + core::Dtype int_dtype_custom = core::Int32; + + // Test with custom parameters. + t::geometry::TriangleMesh ellipsoid_custom = + t::geometry::TriangleMesh::CreateEllipsoid( + 1, 1, 1, 3, float_dtype_custom, int_dtype_custom, device); + + core::Tensor vertex_positions_custom = + core::Tensor::Init({{0.0, 0.0, 1.0}, + {0.0, 0.0, -1.0}, + {0.866025, 0, 0.5}, + {0.433013, 0.75, 0.5}, + {-0.433013, 0.75, 0.5}, + {-0.866025, 0.0, 0.5}, + {-0.433013, -0.75, 0.5}, + {0.433013, -0.75, 0.5}, + {0.866025, 0.0, -0.5}, + {0.433013, 0.75, -0.5}, + {-0.433013, 0.75, -0.5}, + {-0.866025, 0.0, -0.5}, + {-0.433013, -0.75, -0.5}, + {0.433013, -0.75, -0.5}}, + device); + + core::Tensor triangle_indices_custom = core::Tensor::Init( + {{0, 2, 3}, {1, 9, 8}, {0, 3, 4}, {1, 10, 9}, {0, 4, 5}, + {1, 11, 10}, {0, 5, 6}, {1, 12, 11}, {0, 6, 7}, {1, 13, 12}, + {0, 7, 2}, {1, 8, 13}, {8, 3, 2}, {8, 9, 3}, {9, 4, 3}, + {9, 10, 4}, {10, 5, 4}, {10, 11, 5}, {11, 6, 5}, {11, 12, 6}, + {12, 7, 6}, {12, 13, 7}, {13, 2, 7}, {13, 8, 2}}, + device); + + EXPECT_TRUE(ellipsoid_custom.GetVertexPositions().AllClose( + vertex_positions_custom)); + EXPECT_TRUE(ellipsoid_custom.GetTriangleIndices().AllClose( + triangle_indices_custom)); +} + TEST_P(TriangleMeshPermuteDevices, CreateTetrahedron) { core::Device device = GetParam(); core::Dtype float_dtype_custom = core::Float64;