Merge pull request #10 from aalbaali/6-replace-add-with-insert-transform

Use InsertTransform instead of AddTransform

README.md

@@ -64,9 +64,9 @@ std::ostream& operator<<(std::ostream& os, const Frame& frame) {
 
 int main() {
   tg::TransformsGraph<Pose, Frame> 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));
 }
 ```
 
@@ -78,7 +78,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<Transform> 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.
@@ -95,10 +95,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.

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<Pose, Frame> 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, std::string> 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<char>);

examples/minimal_graph_example.cpp

@@ -44,12 +44,12 @@ int main(int argc, char* argv[]) {
   // Construct a graph that consists of two unconnected subgraphs
   auto displacement_inverse = [](const Transform& t) -> Transform { return -t.x(); };
   tg::TransformsGraph<Transform, Frame> transforms(100, displacement_inverse);
-  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;
   }

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<Transform, Frame> 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;
   }

include/transforms_graph/transforms_graph.h

@@ -289,7 +289,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
@@ -454,7 +454,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

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<Displacement, EnumClassFrame> ConstructTwoSubgraphsEnumFrame() {
   tg::TransformsGraph<Displacement, EnumClassFrame> 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<Displacement, EnumClassFrame> ConstructTwoSubgraphsEnumFrame
 tg::TransformsGraph<Displacement, EnumFrame> ConstructTwoSubgraphsFrame() {
   tg::TransformsGraph<Displacement, EnumFrame> 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<Displacement, EnumFrame> ConstructTwoSubgraphsFrame() {
 tg::TransformsGraph<Displacement, int> ConstructTwoSubgraphsIntFrame() {
   tg::TransformsGraph<Displacement, int> 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<Displacement, int> ConstructTwoSubgraphsIntFrame() {
 tg::TransformsGraph<Displacement, size_t> ConstructTwoSubgraphsSizetFrame() {
   tg::TransformsGraph<Displacement, size_t> 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);
 }