Camera coordinate frame visualization with LineSet

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,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<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}});
+
+ // Convert Eigen data to Tensors
+ 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,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_;

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