From 06206af5467a35db32f6fb42cf374461ba6392f1 Mon Sep 17 00:00:00 2001 From: aalbaali Date: Wed, 8 Nov 2023 13:58:19 -0500 Subject: [PATCH] Use InsertTransform instead of AddTransform - Fix #6 Signed-off-by: aalbaali --- README.md | 16 +++---- examples/eigen_pose_example.cpp | 8 ++-- examples/minimal_graph_example.cpp | 12 +++--- examples/sophus_pose_example.cpp | 12 +++--- include/transforms_graph/transforms_graph.h | 4 +- tests/test_transforms_graph.cpp | 46 ++++++++++----------- 6 files changed, 49 insertions(+), 49 deletions(-) diff --git a/README.md b/README.md index b8e9a8f..31d39c1 100644 --- a/README.md +++ b/README.md @@ -63,9 +63,9 @@ std::ostream& operator<<(std::ostream& os, const Frame& frame) { int main() { tg::TransformsGraph transforms; - transforms.AddTransform(Frame::MAP, Frame::ODOM, Pose({1, 2}, 0.0)); - transforms.AddTransform(Frame::BASE_LINK, Frame::CAMERA, Pose({0.5, 0.0}, 0.0)); - transforms.AddTransform(Frame::BASE_LINK, Frame::FRONT_LIDAR, Pose({2, 1}, M_PI_4)); + transforms.InsertTransform(Frame::MAP, Frame::ODOM, Pose({1, 2}, 0.0)); + transforms.InsertTransform(Frame::BASE_LINK, Frame::CAMERA, Pose({0.5, 0.0}, 0.0)); + transforms.InsertTransform(Frame::BASE_LINK, Frame::FRONT_LIDAR, Pose({2, 1}, M_PI_4)); } ``` @@ -77,7 +77,7 @@ This has a minor side effect that the transform could be stored in a way that's For example, if the user does ```c++ tg::TransformsGraph transforms; -transforms.AddTransform('b', 'a', Transform(M_PI_4, {0, 0})); +transforms.InsertTransform('b', 'a', Transform(M_PI_4, {0, 0})); ``` Then, since `'b' > 'a'`, then the transform is inverted and is stored as `T_a_b = Transform(M_PI_4, {0, 0}).inverse()`. When the user asks for the transform `T_b_a`, then the transform is computed using `T_b_a = T_a_b.inverse()` and is returned to the user. @@ -94,10 +94,10 @@ Check the [Transforms storage](#transforms-storage) for additional details on th Taking the output from the [Sophus example](examples/sophus_pose_example.cpp) example ```c++ -transforms.AddTransform(Frame::A, Frame::B, Transform(M_PI_4, {0, 0})); -transforms.AddTransform(Frame::A, Frame::C, Transform(M_PI_2, {0, 0})); -transforms.AddTransform(Frame::B, Frame::D, Transform(0, {1, 2})); -transforms.AddTransform(Frame::E, Frame::F, Transform(-M_PI_4, {3, 4})); +transforms.InsertTransform(Frame::A, Frame::B, Transform(M_PI_4, {0, 0})); +transforms.InsertTransform(Frame::A, Frame::C, Transform(M_PI_2, {0, 0})); +transforms.InsertTransform(Frame::B, Frame::D, Transform(0, {1, 2})); +transforms.InsertTransform(Frame::E, Frame::F, Transform(-M_PI_4, {3, 4})); std::cout << transforms.GetMermaidGraph() << std::endl; ``` and wrapping it in a Markdown code block with `mermaid` language identifier, results in the following flowchart. diff --git a/examples/eigen_pose_example.cpp b/examples/eigen_pose_example.cpp index c283213..9d24c3d 100644 --- a/examples/eigen_pose_example.cpp +++ b/examples/eigen_pose_example.cpp @@ -55,9 +55,9 @@ std::ostream& operator<<(std::ostream& os, const Frame& frame) { int main(int argc, char* argv[]) { tg::TransformsGraph transforms; - transforms.AddTransform(Frame::MAP, Frame::ODOM, Pose({1, 2}, 0.0)); - transforms.AddTransform(Frame::BASE_LINK, Frame::CAMERA, Pose({0.5, 0.0}, 0.0)); - transforms.AddTransform(Frame::BASE_LINK, Frame::FRONT_LIDAR, Pose({2, 1}, M_PI_4)); + transforms.InsertTransform(Frame::MAP, Frame::ODOM, Pose({1, 2}, 0.0)); + transforms.InsertTransform(Frame::BASE_LINK, Frame::CAMERA, Pose({0.5, 0.0}, 0.0)); + transforms.InsertTransform(Frame::BASE_LINK, Frame::FRONT_LIDAR, Pose({2, 1}, M_PI_4)); std::unordered_map frame_names = {{Frame::MAP, "Map"}, {Frame::ODOM, "Odom"}, @@ -71,7 +71,7 @@ int main(int argc, char* argv[]) { << std::endl; // Add transform between already-existing frames - transforms.AddTransform(Frame::ODOM, Frame::BASE_LINK, Pose({-1, 3}, M_PI_2)); + transforms.InsertTransform(Frame::ODOM, Frame::BASE_LINK, Pose({-1, 3}, M_PI_2)); // Set BFS as the graph search algorithm // transforms.SetGraphSearchCallback(tg::BFS); diff --git a/examples/minimal_graph_example.cpp b/examples/minimal_graph_example.cpp index a950ddd..bfe8e79 100644 --- a/examples/minimal_graph_example.cpp +++ b/examples/minimal_graph_example.cpp @@ -44,12 +44,12 @@ int main(int argc, char* argv[]) { // Construct a graph that consists of two unconnected subgraphs tg::TransformsGraph transforms; - transforms.AddTransform('a', 'b', 1); - transforms.AddTransform('a', 'c', 2); - transforms.AddTransform('b', 'd', 3); + transforms.InsertTransform('a', 'b', 1); + transforms.InsertTransform('a', 'c', 2); + transforms.InsertTransform('b', 'd', 3); // Add 'e' and 'f' such that they are in a subgraph that is not connected to the rest of the graph - transforms.AddTransform('e', 'f', 4); + transforms.InsertTransform('e', 'f', 4); // Visualize the graph using mermaid graph. Copy the output and run on // https://mermaid-js.github.io/mermaid-live-editor @@ -62,7 +62,7 @@ int main(int argc, char* argv[]) { std::cout << "Does a->f exist? " << transforms.HasTransform('a', 'f') << std::endl; // Now let's connect the two subgraphs - transforms.AddTransform('c', 'f', 5); + transforms.InsertTransform('c', 'f', 5); // Visualize the graph with edges std::cout << transforms.GetMermaidGraph(true) << std::endl; @@ -80,7 +80,7 @@ int main(int argc, char* argv[]) { // This is because the graph is acyclic and adding a transform to an existing path would // create a cycle try { - transforms.AddTransform('a', 'f', 7); + transforms.InsertTransform('a', 'f', 7); } catch (const std::runtime_error& e) { std::cout << "Caught exception: " << e.what() << std::endl; } diff --git a/examples/sophus_pose_example.cpp b/examples/sophus_pose_example.cpp index 64f92ab..6a599a3 100644 --- a/examples/sophus_pose_example.cpp +++ b/examples/sophus_pose_example.cpp @@ -31,13 +31,13 @@ std::ostream& operator<<(std::ostream& os, const Frame& frame) { int main(int argc, char* argv[]) { // Construct a graph that consists of two unconnected subgraphs tg::TransformsGraph transforms; - transforms.AddTransform(Frame::A, Frame::B, Transform(M_PI_4, {0, 0})); - transforms.AddTransform(Frame::A, Frame::C, Transform(M_PI_2, {0, 0})); - transforms.AddTransform(Frame::B, Frame::D, Transform(0, {1, 2})); + transforms.InsertTransform(Frame::A, Frame::B, Transform(M_PI_4, {0, 0})); + transforms.InsertTransform(Frame::A, Frame::C, Transform(M_PI_2, {0, 0})); + transforms.InsertTransform(Frame::B, Frame::D, Transform(0, {1, 2})); // Add Frame::E and Frame::F such that they are in a subgraph that is not connected to the rest of // the graph - transforms.AddTransform(Frame::E, Frame::F, Transform(-M_PI_4, {3, 4})); + transforms.InsertTransform(Frame::E, Frame::F, Transform(-M_PI_4, {3, 4})); // Resolve a vector in a different frame const auto T_a_b = transforms.GetTransform(Frame::A, Frame::B); @@ -63,7 +63,7 @@ int main(int argc, char* argv[]) { std::cout << "Does a->f exist? " << transforms.HasTransform(Frame::A, Frame::F) << std::endl; // Now let's connect the two subgraphs - transforms.AddTransform(Frame::C, Frame::F, Transform(0, {1, 0})); + transforms.InsertTransform(Frame::C, Frame::F, Transform(0, {1, 0})); // Visualize the graph with edges std::cout << transforms.GetMermaidGraph(true) << std::endl; @@ -81,7 +81,7 @@ int main(int argc, char* argv[]) { // This is because the graph is acyclic and adding a transform to an existing path would // create a cycle try { - transforms.AddTransform(Frame::A, Frame::F, Transform(0, {0, 1})); + transforms.InsertTransform(Frame::A, Frame::F, Transform(0, {0, 1})); } catch (const std::runtime_error& e) { std::cout << "Caught exception: " << e.what() << std::endl; } diff --git a/include/transforms_graph/transforms_graph.h b/include/transforms_graph/transforms_graph.h index df17b47..4fb28fd 100644 --- a/include/transforms_graph/transforms_graph.h +++ b/include/transforms_graph/transforms_graph.h @@ -278,7 +278,7 @@ class TransformsGraph { * @param[in] pose Transform from parent to child. That is, for a displacement `r_child` resolved * in the `child` frame, it can be resolved in the parent frame using `r_parent = pose * r_child` */ - void AddTransform(Frame parent, Frame child, Transform&& pose, bool should_override = false) { + void InsertTransform(Frame parent, Frame child, Transform&& pose, bool should_override = false) { // Handle the case where the transform exists. The only situation in which the transform is // overridden is if the override flag is set tot true AND the transform to be updated is a raw // transform @@ -442,7 +442,7 @@ class TransformsGraph { * in the graph. * * @details This function is used internally and is not expected to be exposed to the user. The - * user should instead use the `AddTransform` function. + * user should instead use the `InsertTransform` function. * * @param[in] parent Parent frame * @param[in] child Child frame diff --git a/tests/test_transforms_graph.cpp b/tests/test_transforms_graph.cpp index 48a003f..698e5ec 100644 --- a/tests/test_transforms_graph.cpp +++ b/tests/test_transforms_graph.cpp @@ -54,12 +54,12 @@ TransformsGraph ConstructTwoSubgraphs() { /// e -> f TransformsGraph transforms; // First connected subgraph - transforms.AddTransform('a', 'b', 1); - transforms.AddTransform('a', 'c', 2); - transforms.AddTransform('b', 'd', 3); + transforms.InsertTransform('a', 'b', 1); + transforms.InsertTransform('a', 'c', 2); + transforms.InsertTransform('b', 'd', 3); // Second connected subgraph - transforms.AddTransform('e', 'f', 4); + transforms.InsertTransform('e', 'f', 4); return transforms; } @@ -75,12 +75,12 @@ enum EnumFrame { A, B, C, D, E, F }; tg::TransformsGraph ConstructTwoSubgraphsEnumFrame() { tg::TransformsGraph transforms; // First connected subgraph - transforms.AddTransform(EnumClassFrame::a, EnumClassFrame::b, 1); - transforms.AddTransform(EnumClassFrame::a, EnumClassFrame::c, 2); - transforms.AddTransform(EnumClassFrame::b, EnumClassFrame::d, 3); + transforms.InsertTransform(EnumClassFrame::a, EnumClassFrame::b, 1); + transforms.InsertTransform(EnumClassFrame::a, EnumClassFrame::c, 2); + transforms.InsertTransform(EnumClassFrame::b, EnumClassFrame::d, 3); // Second connected subgraph - transforms.AddTransform(EnumClassFrame::e, EnumClassFrame::f, 4); + transforms.InsertTransform(EnumClassFrame::e, EnumClassFrame::f, 4); return transforms; } @@ -92,12 +92,12 @@ tg::TransformsGraph ConstructTwoSubgraphsEnumFrame tg::TransformsGraph ConstructTwoSubgraphsFrame() { tg::TransformsGraph transforms; // First connected subgraph - transforms.AddTransform(A, B, 1); - transforms.AddTransform(A, C, 2); - transforms.AddTransform(B, D, 3); + transforms.InsertTransform(A, B, 1); + transforms.InsertTransform(A, C, 2); + transforms.InsertTransform(B, D, 3); // Second connected subgraph - transforms.AddTransform(E, F, 4); + transforms.InsertTransform(E, F, 4); return transforms; } @@ -109,12 +109,12 @@ tg::TransformsGraph ConstructTwoSubgraphsFrame() { tg::TransformsGraph ConstructTwoSubgraphsIntFrame() { tg::TransformsGraph transforms; // First connected subgraph - transforms.AddTransform(1, 2, 1); - transforms.AddTransform(1, 3, 2); - transforms.AddTransform(2, 4, 3); + transforms.InsertTransform(1, 2, 1); + transforms.InsertTransform(1, 3, 2); + transforms.InsertTransform(2, 4, 3); // Second connected subgraph - transforms.AddTransform(5, 6, 4); + transforms.InsertTransform(5, 6, 4); return transforms; } @@ -126,12 +126,12 @@ tg::TransformsGraph ConstructTwoSubgraphsIntFrame() { tg::TransformsGraph ConstructTwoSubgraphsSizetFrame() { tg::TransformsGraph transforms; // First connected subgraph - transforms.AddTransform(1, 2, 1); - transforms.AddTransform(1, 3, 2); - transforms.AddTransform(2, 4, 3); + transforms.InsertTransform(1, 2, 1); + transforms.InsertTransform(1, 3, 2); + transforms.InsertTransform(2, 4, 3); // Second connected subgraph - transforms.AddTransform(5, 6, 4); + transforms.InsertTransform(5, 6, 4); return transforms; } @@ -218,7 +218,7 @@ TEST(TransformsGraph, ChainedTransforms) { TEST(TransformsGraph, AddingTransform) { // Add a transform that connects the two subgraphs auto transforms = ConstructTwoSubgraphs(); - transforms.AddTransform('d', 'e', 4); + transforms.InsertTransform('d', 'e', 4); // a<->e chained transform should exist EXPECT_TRUE(transforms.HasTransform('a', 'e')); @@ -236,10 +236,10 @@ TEST(TransformsGraph, ExceedingMaxFrames) { EXPECT_EQ(transforms.GetAllFrames().size(), 3); EXPECT_ANY_THROW(transforms.AddFrame('d')); - EXPECT_ANY_THROW(transforms.AddTransform('a', 'd', 5)); + EXPECT_ANY_THROW(transforms.InsertTransform('a', 'd', 5)); // Adding a transform to an already-existing transforms shouldn't throw an exception - EXPECT_NO_THROW(transforms.AddTransform('a', 'b', 1)); + EXPECT_NO_THROW(transforms.InsertTransform('a', 'b', 1)); EXPECT_EQ(transforms.GetAllRawTransforms().size(), 1); }