Robert/add pose to triad API

flatbuffers/geometry.fbs

@@ -5,6 +5,7 @@ namespace slamd.flatb;
 table Triad {
     scale: float;
     thickness: float;
+    pose: Mat4;
 }
 
 table Circles2D {

flatbuffers/messages.fbs

@@ -56,8 +56,6 @@ table UpdatePointCloudRadii {
     radii: [float];
 }
 
-
-
 // general updates
 table SetTransform {
     tree_id: ulong;

python_bindings/pyproject.toml

@@ -4,7 +4,7 @@ build-backend = "scikit_build_core.build"
 
 [project]
 name = "slamd"
-version = "2.1.11"
+version = "2.1.12"
 description = "Python bindings for SlamDunk"
 authors = [{ name = "Robert Leo", email = "robert.leo.jonsson@gmail.com" }]
 readme = "README.md"

python_bindings/src/main.cpp

@@ -576,7 +576,14 @@ void define_geom(
 
     m.def(
         "Triad",
-        &slamd::geom::triad,
+        [](std::optional<glm::mat4> pose, float scale, float thickness) {
+            if (pose.has_value()) {
+                return slamd::geom::triad(pose.value(), scale, thickness);
+            } else {
+                return slamd::geom::triad(scale, thickness);
+            }
+        },
+        py::arg("pose") = std::nullopt,
         py::arg("scale") = 1.0f,
         py::arg("thickness") = 0.1f,
         "Create a Triad geometry"

python_bindings/src/slamd/bindings/geom.pyi

@@ -42,7 +42,7 @@ def Sphere(radius: typing.SupportsFloat = 1.0, color: numpy.ndarray = ...) -> bi
     """
     Create a Sphere geometry
     """
-def Triad(scale: typing.SupportsFloat = 1.0, thickness: typing.SupportsFloat = 0.10000000149011612) -> bindings._geom.Triad:
+def Triad(pose: numpy.ndarray[float32[4][4]] | None = None, scale: typing.SupportsFloat = 1.0, thickness: typing.SupportsFloat = 0.10000000149011612) -> bindings._geom.Triad:
     """
     Create a Triad geometry
     """

python_bindings/src/slamd/geom/overrides.py

@@ -5,6 +5,7 @@
     Sphere as Sphere_internal,
     Arrows as Arrows_internal,
     Plane as Plane_internal,
+    Triad as Triad_internal,
 )
 from .._utils.colors import Color
 from .._utils.handle_input import process_color, process_radii, process_single_color
@@ -95,3 +96,7 @@ def Plane(
     alpha: float = 0.8,
 ):
     return Plane_internal(normal, point, process_single_color(color), radius, alpha)
+
+
+def Triad(pose: np.ndarray | None = None, scale: float = 1.0, thickness: float = 1.0):
+    return Triad_internal(pose, scale, thickness)

python_examples/bunch_of_triads.py

@@ -0,0 +1,39 @@
+import slamd
+import numpy as np
+
+
+def random_poses(n: int, t_scale: float = 10.0) -> np.ndarray:
+    poses = []
+    for _ in range(n):
+        M = np.random.randn(3, 3)
+        Q, R = np.linalg.qr(M)
+        if np.linalg.det(Q) < 0:
+            Q[:, 0] *= -1
+
+        t = np.random.uniform(-t_scale, t_scale, size=(3,))
+
+        T = np.eye(4)
+        T[:3, :3] = Q
+        T[:3, 3] = t
+        poses.append(T)
+
+    return np.stack(poses, axis=0)
+
+
+def main():
+    vis = slamd.Visualizer("poses")
+    scene = vis.scene("poses")
+
+    for i, pose_mat in enumerate(random_poses(100, 10)):
+        scene.set_object(f"/triad_{i}", slamd.geom.Triad(pose_mat))
+
+    last_pose_mat = np.eye(4)
+    last_pose_mat[0, 3] = 20
+
+    scene.set_object("/triad_x", slamd.geom.Triad(last_pose_mat, 10))
+
+    vis.hang_forever()
+
+
+if __name__ == "__main__":
+    main()

slamd/flatb/flatb/geometry_generated.h

@@ -189,18 +189,23 @@ struct Triad FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table {
   typedef TriadBuilder Builder;
   enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE {
     VT_SCALE = 4,
-    VT_THICKNESS = 6
+    VT_THICKNESS = 6,
+    VT_POSE = 8
   };
   float scale() const {
     return GetField<float>(VT_SCALE, 0.0f);
   }
   float thickness() const {
     return GetField<float>(VT_THICKNESS, 0.0f);
   }
+  const slamd::flatb::Mat4 *pose() const {
+    return GetStruct<const slamd::flatb::Mat4 *>(VT_POSE);
+  }
   bool Verify(::flatbuffers::Verifier &verifier) const {
     return VerifyTableStart(verifier) &&
            VerifyField<float>(verifier, VT_SCALE, 4) &&
            VerifyField<float>(verifier, VT_THICKNESS, 4) &&
+           VerifyField<slamd::flatb::Mat4>(verifier, VT_POSE, 4) &&
            verifier.EndTable();
   }
 };
@@ -215,6 +220,9 @@ struct TriadBuilder {
   void add_thickness(float thickness) {
     fbb_.AddElement<float>(Triad::VT_THICKNESS, thickness, 0.0f);
   }
+  void add_pose(const slamd::flatb::Mat4 *pose) {
+    fbb_.AddStruct(Triad::VT_POSE, pose);
+  }
   explicit TriadBuilder(::flatbuffers::FlatBufferBuilder &_fbb)
         : fbb_(_fbb) {
     start_ = fbb_.StartTable();
@@ -229,8 +237,10 @@ struct TriadBuilder {
 inline ::flatbuffers::Offset<Triad> CreateTriad(
     ::flatbuffers::FlatBufferBuilder &_fbb,
     float scale = 0.0f,
-    float thickness = 0.0f) {
+    float thickness = 0.0f,
+    const slamd::flatb::Mat4 *pose = nullptr) {
   TriadBuilder builder_(_fbb);
+  builder_.add_pose(pose);
   builder_.add_thickness(thickness);
   builder_.add_scale(scale);
   return builder_.Finish();

slamd/include/slamd/geom/triad.hpp

@@ -8,7 +8,11 @@ namespace _geom {
 
 class Triad : public Geometry {
    public:
-    Triad(float scale, float thickness);
+    Triad(
+        float scale,
+        float thickness,
+        std::optional<glm::mat4> pose = std::nullopt
+    );
 
     flatbuffers::Offset<slamd::flatb::Geometry> serialize(
         flatbuffers::FlatBufferBuilder& builder
@@ -17,6 +21,7 @@ class Triad : public Geometry {
    private:
     float scale;
     float thickness;
+    glm::mat4 pose;
 };
 
 }  // namespace _geom
@@ -26,6 +31,7 @@ namespace geom {
 using TriadPtr = std::shared_ptr<_geom::Triad>;
 
 TriadPtr triad(float scale = 1.0, float thickness = 0.1);
+TriadPtr triad(glm::mat4 pose, float scale = 1.0, float thickness = 0.1);
 
 }  // namespace geom
 

slamd/include/slamd_window/geom/triad.hpp

@@ -11,7 +11,7 @@ namespace _geom {
 
 class Triad : public Geometry {
    public:
-    Triad(float scale, float thickness);
+    Triad(float scale, float thickness, glm::mat4 pose);
 
     void render(glm::mat4 model, glm::mat4 view, glm::mat4 projection) override;
 
@@ -21,6 +21,7 @@ class Triad : public Geometry {
 
    private:
     std::unique_ptr<Arrows> arrows;
+    glm::mat4 pose;
     glm::mat4 scale_transform;
 };
 
@@ -30,7 +31,11 @@ namespace geom {
 
 using TriadPtr = std::shared_ptr<_geom::Triad>;
 
-TriadPtr triad(float scale = 1.0, float thickness = 0.1);
+TriadPtr triad(
+    float scale = 1.0,
+    float thickness = 0.1,
+    glm::mat4 pose = glm::mat4(1.0)
+);
 
 }  // namespace geom
 

slamd/src/slamd/geom/triad.cpp

@@ -1,21 +1,26 @@
 #include <slamd/geom/triad.hpp>
-
+#include <slamd_common/gmath/serialization.hpp>
 #include <slamd_common/gmath/transforms.hpp>
 
 namespace slamd {
 namespace _geom {
 
 Triad::Triad(
     float scale,
-    float thickness
+    float thickness,
+    std::optional<glm::mat4> pose
 )
     : scale(scale),
-      thickness(thickness) {}
+      thickness(thickness),
+      pose(pose.value_or(glm::mat4(1.0))) {}
 
 flatbuffers::Offset<slamd::flatb::Geometry> Triad::serialize(
     flatbuffers::FlatBufferBuilder& builder
 ) {
-    auto triad_fb = flatb::CreateTriad(builder, this->scale, this->thickness);
+    auto pose_fb = gmath::serialize(pose);
+
+    auto triad_fb =
+        flatb::CreateTriad(builder, this->scale, this->thickness, &pose_fb);
 
     return flatb::CreateGeometry(
         builder,
@@ -37,6 +42,16 @@ TriadPtr triad(
     // _global::geometries.add(triad->id, triad);
     return triad;
 }
+
+TriadPtr triad(
+    glm::mat4 pose,
+    float scale,
+    float thickness
+) {
+    auto triad = std::make_shared<_geom::Triad>(scale, thickness, pose);
+    // _global::geometries.add(triad->id, triad);
+    return triad;
+}
 }  // namespace geom
 
 }  // namespace slamd

slamd/src/window/geom/triad.cpp

@@ -1,3 +1,4 @@
+#include <slamd_common/gmath/serialization.hpp>
 #include <slamd_common/gmath/transforms.hpp>
 #include <slamd_window/geom/triad.hpp>
 
@@ -7,7 +8,13 @@ namespace _geom {
 std::shared_ptr<Triad> Triad::deserialize(
     const slamd::flatb::Triad* triad_fb
 ) {
-    return std::make_shared<Triad>(triad_fb->scale(), triad_fb->thickness());
+    glm::mat4 pose = gmath::deserialize(triad_fb->pose());
+
+    return std::make_shared<Triad>(
+        triad_fb->scale(),
+        triad_fb->thickness(),
+        pose
+    );
 }
 
 std::unique_ptr<Arrows> make_arrows(
@@ -25,9 +32,11 @@ std::unique_ptr<Arrows> make_arrows(
 
 Triad::Triad(
     float scale,
-    float thickness
+    float thickness,
+    glm::mat4 pose
 )
-    : arrows(make_arrows(thickness)) {
+    : arrows(make_arrows(thickness)),
+      pose(pose) {
     this->scale_transform = slamd::gmath::scale(glm::vec3(scale, scale, scale));
 }
 
@@ -36,7 +45,8 @@ void Triad::render(
     glm::mat4 view,
     glm::mat4 projection
 ) {
-    this->arrows->render(model * this->scale_transform, view, projection);
+    this->arrows
+        ->render(model * this->pose * this->scale_transform, view, projection);
 }
 
 }  // namespace _geom
@@ -45,9 +55,10 @@ namespace geom {
 
 TriadPtr triad(
     float scale,
-    float thickness
+    float thickness,
+    glm::mat4 pose
 ) {
-    return std::make_shared<_geom::Triad>(scale, thickness);
+    return std::make_shared<_geom::Triad>(scale, thickness, pose);
 }
 }  // namespace geom