Camera coordinate frame visualization with LineSet (#6796)

cpp/open3d/geometry/LineSetFactory.cpp

@@ -170,6 +170,25 @@ std::shared_ptr<LineSet> 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;
 }
 

cpp/open3d/t/geometry/LineSet.cpp

@@ -9,6 +9,7 @@
 
 #include <string>
 
+#include "open3d/core/Dtype.h"
 #include "open3d/core/EigenConverter.h"
 #include "open3d/core/ShapeUtil.h"
 #include "open3d/core/Tensor.h"
@@ -211,6 +212,81 @@ 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, "CPU:0");
+    core::Tensor extrinsic = extrinsic_in.To(core::Float32, "CPU:0");
+
+    // Calculate points for camera visualization
+    float w(view_width_px), h(view_height_px), s(scale);
+    float fx = intrinsic[0][0].Item<float>(),
+          fy = intrinsic[1][1].Item<float>(),
+          cx = intrinsic[0][2].Item<float>(),
+          cy = intrinsic[1][2].Item<float>();
+    core::Tensor points = core::Tensor::Init<float>({{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<int>({{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}});
+
+    LineSet lineset(points, lines);
+    if (color.NumElements() == 3) {
+        lineset.PaintUniformColor(color);
+    } else {
+        lineset.PaintUniformColor(core::Tensor::Init<float>({0.f, 0.f, 1.f}));
+    }
+    auto &lscolors = lineset.GetLineColors();
+    lscolors[8] = core::Tensor::Init<float>({1.f, 0.f, 0.f});   // Red
+    lscolors[9] = core::Tensor::Init<float>({0.f, 1.f, 0.f});   // Green
+    lscolors[10] = core::Tensor::Init<float>({0.f, 0.f, 1.f});  // Blue
+
+    return lineset;
+}
+
 }  // namespace geometry
 }  // namespace t
 }  // namespace open3d

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 &center);
 
+    /// \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,22 @@ 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_;

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])
 )");
 }