diff --git a/flatbuffers/geometry.fbs b/flatbuffers/geometry.fbs index 103e36e..6339faf 100644 --- a/flatbuffers/geometry.fbs +++ b/flatbuffers/geometry.fbs @@ -5,6 +5,7 @@ namespace slamd.flatb; table Triad { scale: float; thickness: float; + pose: Mat4; } table Circles2D { diff --git a/flatbuffers/messages.fbs b/flatbuffers/messages.fbs index 24ab740..f916e0e 100644 --- a/flatbuffers/messages.fbs +++ b/flatbuffers/messages.fbs @@ -56,8 +56,6 @@ table UpdatePointCloudRadii { radii: [float]; } - - // general updates table SetTransform { tree_id: ulong; diff --git a/python_bindings/pyproject.toml b/python_bindings/pyproject.toml index 84cb800..b5d8425 100644 --- a/python_bindings/pyproject.toml +++ b/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" diff --git a/python_bindings/src/main.cpp b/python_bindings/src/main.cpp index bea3400..74075cc 100644 --- a/python_bindings/src/main.cpp +++ b/python_bindings/src/main.cpp @@ -576,7 +576,14 @@ void define_geom( m.def( "Triad", - &slamd::geom::triad, + [](std::optional 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" diff --git a/python_bindings/src/slamd/bindings/geom.pyi b/python_bindings/src/slamd/bindings/geom.pyi index fb0a96c..fab1f7c 100644 --- a/python_bindings/src/slamd/bindings/geom.pyi +++ b/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 """ diff --git a/python_bindings/src/slamd/geom/overrides.py b/python_bindings/src/slamd/geom/overrides.py index 6d72710..c1aa99b 100644 --- a/python_bindings/src/slamd/geom/overrides.py +++ b/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) diff --git a/python_examples/bunch_of_triads.py b/python_examples/bunch_of_triads.py new file mode 100644 index 0000000..1fd943b --- /dev/null +++ b/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() diff --git a/slamd/flatb/flatb/geometry_generated.h b/slamd/flatb/flatb/geometry_generated.h index 05d0026..3f63ca6 100644 --- a/slamd/flatb/flatb/geometry_generated.h +++ b/slamd/flatb/flatb/geometry_generated.h @@ -189,7 +189,8 @@ 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(VT_SCALE, 0.0f); @@ -197,10 +198,14 @@ struct Triad FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table { float thickness() const { return GetField(VT_THICKNESS, 0.0f); } + const slamd::flatb::Mat4 *pose() const { + return GetStruct(VT_POSE); + } bool Verify(::flatbuffers::Verifier &verifier) const { return VerifyTableStart(verifier) && VerifyField(verifier, VT_SCALE, 4) && VerifyField(verifier, VT_THICKNESS, 4) && + VerifyField(verifier, VT_POSE, 4) && verifier.EndTable(); } }; @@ -215,6 +220,9 @@ struct TriadBuilder { void add_thickness(float thickness) { fbb_.AddElement(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 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(); diff --git a/slamd/include/slamd/geom/triad.hpp b/slamd/include/slamd/geom/triad.hpp index 0ad1e86..80e5306 100644 --- a/slamd/include/slamd/geom/triad.hpp +++ b/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 pose = std::nullopt + ); flatbuffers::Offset 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 diff --git a/slamd/include/slamd_window/geom/triad.hpp b/slamd/include/slamd_window/geom/triad.hpp index 789a753..21bd746 100644 --- a/slamd/include/slamd_window/geom/triad.hpp +++ b/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; + 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 diff --git a/slamd/src/slamd/geom/triad.cpp b/slamd/src/slamd/geom/triad.cpp index 4edc311..6becbd1 100644 --- a/slamd/src/slamd/geom/triad.cpp +++ b/slamd/src/slamd/geom/triad.cpp @@ -1,5 +1,5 @@ #include - +#include #include namespace slamd { @@ -7,15 +7,20 @@ namespace _geom { Triad::Triad( float scale, - float thickness + float thickness, + std::optional pose ) : scale(scale), - thickness(thickness) {} + thickness(thickness), + pose(pose.value_or(glm::mat4(1.0))) {} flatbuffers::Offset 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 \ No newline at end of file diff --git a/slamd/src/window/geom/triad.cpp b/slamd/src/window/geom/triad.cpp index 8339780..7ab8324 100644 --- a/slamd/src/window/geom/triad.cpp +++ b/slamd/src/window/geom/triad.cpp @@ -1,3 +1,4 @@ +#include #include #include @@ -7,7 +8,13 @@ namespace _geom { std::shared_ptr Triad::deserialize( const slamd::flatb::Triad* triad_fb ) { - return std::make_shared(triad_fb->scale(), triad_fb->thickness()); + glm::mat4 pose = gmath::deserialize(triad_fb->pose()); + + return std::make_shared( + triad_fb->scale(), + triad_fb->thickness(), + pose + ); } std::unique_ptr make_arrows( @@ -25,9 +32,11 @@ std::unique_ptr 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