From dbe287085f9742b1c5404e7351fd6dc62306e61d Mon Sep 17 00:00:00 2001 From: Sameer Sheorey Date: Mon, 20 May 2024 12:23:45 -0700 Subject: [PATCH] Camera coordinate frame visualization with LineSet --- cpp/open3d/geometry/LineSetFactory.cpp | 19 +++++++ cpp/open3d/t/geometry/LineSet.cpp | 77 ++++++++++++++++++++++++++ cpp/open3d/t/geometry/LineSet.h | 23 ++++++++ cpp/pybind/t/geometry/lineset.cpp | 31 +++++++++++ 4 files changed, 150 insertions(+) diff --git a/cpp/open3d/geometry/LineSetFactory.cpp b/cpp/open3d/geometry/LineSetFactory.cpp index 41e44d1db5f..3a66d86d300 100644 --- a/cpp/open3d/geometry/LineSetFactory.cpp +++ b/cpp/open3d/geometry/LineSetFactory.cpp @@ -170,6 +170,25 @@ std::shared_ptr LineSet::CreateCameraVisualization( lines->lines_.push_back({4, 1}); lines->PaintUniformColor({0.0f, 0.0f, 1.0f}); + // Add XYZ axes + lines->points_.push_back( + mult(m, Eigen::Vector3d{intrinsic(0, 0) * scale, 0.0, 0.0})); + lines->points_.push_back( + mult(m, Eigen::Vector3d{0.0, intrinsic(1, 1) * scale, 0.0})); + lines->points_.push_back( + mult(m, Eigen::Vector3d{intrinsic(0, 2) * scale, + intrinsic(1, 2) * scale, scale})); + + // Add lines for the axes + lines->lines_.push_back({0, 5}); // X axis (red) + lines->lines_.push_back({0, 6}); // Y axis (green) + lines->lines_.push_back({0, 7}); // Z axis (blue) + + // Set colors for the axes + lines->colors_.push_back({1.0f, 0.0f, 0.0f}); // Red + lines->colors_.push_back({0.0f, 1.0f, 0.0f}); // Green + lines->colors_.push_back({0.0f, 0.0f, 1.0f}); // Blue + return lines; } diff --git a/cpp/open3d/t/geometry/LineSet.cpp b/cpp/open3d/t/geometry/LineSet.cpp index c2d95f0ced2..8a11ebc657c 100644 --- a/cpp/open3d/t/geometry/LineSet.cpp +++ b/cpp/open3d/t/geometry/LineSet.cpp @@ -9,6 +9,7 @@ #include +#include "open3d/core/Dtype.h" #include "open3d/core/EigenConverter.h" #include "open3d/core/ShapeUtil.h" #include "open3d/core/Tensor.h" @@ -211,6 +212,82 @@ OrientedBoundingBox LineSet::GetOrientedBoundingBox() const { return OrientedBoundingBox::CreateFromPoints(GetPointPositions()); } +LineSet &LineSet::PaintUniformColor(const core::Tensor &color) { + core::AssertTensorShape(color, {3}); + core::Tensor clipped_color = color.To(GetDevice()); + if (color.GetDtype() == core::Float32 || + color.GetDtype() == core::Float64) { + clipped_color = clipped_color.Clip(0.0f, 1.0f); + } + core::Tensor ls_colors = + core::Tensor::Empty({GetLineIndices().GetLength(), 3}, + clipped_color.GetDtype(), GetDevice()); + ls_colors.AsRvalue() = clipped_color; + SetLineColors(ls_colors); + + return *this; +} + +LineSet LineSet::CreateCameraVisualization(int view_width_px, + int view_height_px, + const core::Tensor &intrinsic_in, + const core::Tensor &extrinsic_in, + double scale, + const core::Tensor &color) { + core::AssertTensorShape(intrinsic_in, {3, 3}); + core::AssertTensorShape(extrinsic_in, {4, 4}); + core::Tensor intrinsic = intrinsic_in.To(core::Float32); + core::Tensor extrinsic = extrinsic_in.To(core::Float32); + + // Calculate points for camera visualization + float w(view_width_px), h(view_height_px), s(scale); + float fx = intrinsic[0][0].Item(), + fy = intrinsic[1][1].Item(), + cx = intrinsic[0][2].Item(), + cy = intrinsic[1][2].Item(); + core::Tensor points = core::Tensor::Init({{0.f, 0.f, 0.f}, // origin + {0.f, 0.f, s}, + {w * s, 0.f, s}, + {w * s, h * s, s}, + {0.f, h * s, s}, + // Add XYZ axes + {fx * s, 0.f, 0.f}, + {0.f, fy * s, 0.f}, + {cx * s, cy * s, s}}); + points = (intrinsic.Inverse().Matmul(points.T()) - + extrinsic.Slice(0, 0, 3).Slice(1, 3, 4)) + .T() + .Matmul(extrinsic.Slice(0, 0, 3).Slice(1, 0, 3)); + + // Add lines for camera frame and XYZ axes + core::Tensor lines = core::Tensor::Init({{0, 1}, + {0, 2}, + {0, 3}, + {0, 4}, + {1, 2}, + {2, 3}, + {3, 4}, + {4, 1}, + // Add XYZ axes + {0, 5}, + {0, 6}, + {0, 7}}); + + // Convert Eigen data to Tensors + LineSet lineset(points, lines); + if (color.NumElements() == 3) { + lineset.PaintUniformColor(color); + } else { + lineset.PaintUniformColor(core::Tensor::Init({0.f, 0.f, 1.f})); + } + auto &lscolors = lineset.GetLineColors(); + lscolors[8] = core::Tensor::Init({1.f, 0.f, 0.f}); // Red + lscolors[9] = core::Tensor::Init({0.f, 1.f, 0.f}); // Green + lscolors[10] = core::Tensor::Init({0.f, 0.f, 1.f}); // Blue + + return lineset; +} + } // namespace geometry } // namespace t } // namespace open3d diff --git a/cpp/open3d/t/geometry/LineSet.h b/cpp/open3d/t/geometry/LineSet.h index 6c3ecb66da8..7d719881788 100644 --- a/cpp/open3d/t/geometry/LineSet.h +++ b/cpp/open3d/t/geometry/LineSet.h @@ -337,6 +337,12 @@ class LineSet : public Geometry, public DrawableGeometry { /// \return Rotated line set. LineSet &Rotate(const core::Tensor &R, const core::Tensor ¢er); + /// \brief Assigns uniform color to all lines of the LineSet. + /// + /// \param color RGB color for the LineSet. {3,} shaped Tensor. + /// Floating color values are clipped between 0.0 and 1.0. + LineSet &PaintUniformColor(const core::Tensor &color); + /// \brief Returns the device attribute of this LineSet. core::Device GetDevice() const override { return device_; } @@ -385,6 +391,23 @@ class LineSet : public Geometry, public DrawableGeometry { double scale = 1.0, bool capping = true) const; + /// Factory function to create a LineSet from intrinsic and extrinsic + /// matrices. + /// + /// \param view_width_px The width of the view, in pixels. + /// \param view_height_px The height of the view, in pixels. + /// \param intrinsic The intrinsic matrix {3,3} shape. + /// \param extrinsic The extrinsic matrix {4,4} shape. + /// \param scale camera scale + /// \param color tensor with float32 dtype and shape {3}. Default is blue. + static LineSet CreateCameraVisualization(int view_width_px, + int view_height_px, + const core::Tensor &intrinsic, + const core::Tensor &extrinsic, + double scale, + const core::Tensor &color = {} + ); + protected: core::Device device_ = core::Device("CPU:0"); TensorMap point_attr_; diff --git a/cpp/pybind/t/geometry/lineset.cpp b/cpp/pybind/t/geometry/lineset.cpp index d9bf8adbe43..299deef36b0 100644 --- a/cpp/pybind/t/geometry/lineset.cpp +++ b/cpp/pybind/t/geometry/lineset.cpp @@ -299,6 +299,37 @@ transformation as :math:`P = R(P) + t`)"); mesh = lines.extrude_linear([0,1,0]) o3d.visualization.draw([{'name': 'L', 'geometry': mesh}]) +)"); + line_set.def("paint_uniform_color", &LineSet::PaintUniformColor, "color"_a, + "Assigns unifom color to all the lines of the LineSet. " + "Floating color values are clipped between 00 and 1.0. Input " + "`color` should be a (3,) shape tensor."); + line_set.def_static( + "create_camera_visualization", &LineSet::CreateCameraVisualization, + "view_width_px"_a, "view_height_px"_a, "intrinsic"_a, "extrinsic"_a, + "scale"_a = 1.f, "color"_a = core::Tensor({}, core::Float32), + R"(Factory function to create a LineSet from intrinsic and extrinsic +matrices. Camera reference frame is shown with XYZ axes in RGB. + +Args: + view_width_px (int): The width of the view, in pixels. + view_height_px (int): The height of the view, in pixels. + intrinsic (open3d.core.Tensor): The intrinsic matrix {3,3} shape. + extrinsic (open3d.core.Tensor): The extrinsic matrix {4,4} shape. + scale (float): camera scale + color (open3d.core.Tensor): color with float32 and shape {3}. Default is blue. + +Example: + + Draw a purple camera frame with XYZ axes in RGB. + + import open3d.core as o3c + from open3d.t.geometry import LineSet + from open3d.visualization import draw + K = o3c.Tensor([[512, 0, 512], [0, 512, 512], [0, 0, 1]], dtype=o3c.float32) + T = o3c.Tensor.eye(4, dtype=o3c.float32) + ls = LineSet.create_camera_visualization(1024, 1024, K, T, 1, [0.8, 0.2, 0.8]) + draw([ls]) )"); }