Feature/merge with transform (#32)

* add transform to merge graph

* (WIP) iterating on interface and purpose

* update unittests

* fix python workflow apt error

* fix cmake workflow apt error

.github/workflows/cmake.yaml

@@ -11,7 +11,7 @@ jobs:
       # setup package dependencies
       - name: Dependencies
         run: |
-          sudo apt install libgtest-dev libeigen3-dev nlohmann-json3-dev libzmq3-dev python3-dev
+          sudo apt-get update && sudo apt install libgtest-dev libeigen3-dev nlohmann-json3-dev libzmq3-dev python3-dev
       # configure repo
       - name: Configure
         run: |

.github/workflows/python.yaml

@@ -20,7 +20,7 @@ jobs:
       - name: Dependencies
         run: |
           python -m pip install --upgrade pip
-          sudo apt install libgtest-dev libeigen3-dev nlohmann-json3-dev libzmq3-dev python3-dev
+          sudo apt-get update && sudo apt install libgtest-dev libeigen3-dev nlohmann-json3-dev libzmq3-dev python3-dev
       # build and install code
       - name: Install
         run: python -m pip install ${{github.workspace}}

include/spark_dsg/dynamic_scene_graph.h

@@ -411,9 +411,12 @@ class DynamicSceneGraph {
    * @note Will add the nodes and edges not previously added in current graph
    * @param other other graph to update from
    * @param config Configuration controlling merge behavior
+   * @param transform the other graph when merging
    * @returns True if merge was successful
    */
-  bool mergeGraph(const DynamicSceneGraph& other, const GraphMergeConfig& config = {});
+  bool mergeGraph(const DynamicSceneGraph& other,
+                  const GraphMergeConfig& config = {},
+                  const Eigen::Affine3d* transform = nullptr);
 
   /**
    * @brief Get all removed nodes from the graph

include/spark_dsg/scene_graph_layer.h

@@ -33,6 +33,7 @@
  * purposes notwithstanding any copyright notation herein.
  * -------------------------------------------------------------------------- */
 #pragma once
+#include <Eigen/Geometry>
 #include <functional>
 #include <map>
 #include <unordered_set>
@@ -232,7 +233,8 @@ class SceneGraphLayer {
    */
   void mergeLayer(const SceneGraphLayer& other,
                   const GraphMergeConfig& config,
-                  std::vector<NodeId>* new_nodes);
+                  std::vector<NodeId>* new_nodes = nullptr,
+                  const Eigen::Affine3d* transform_new_nodes = nullptr);
 
   /**
    * @brief Get node ids of newly inserted nodes

src/dynamic_scene_graph.cpp

@@ -559,7 +559,8 @@ bool DynamicSceneGraph::updateFromLayer(const SceneGraphLayer& other_layer,
 }
 
 bool DynamicSceneGraph::mergeGraph(const DynamicSceneGraph& other,
-                                   const GraphMergeConfig& config) {
+                                   const GraphMergeConfig& config,
+                                   const Eigen::Affine3d* transform_new_nodes) {
   metadata.add(other.metadata());
 
   other.visitLayers([&](LayerKey layer_key, const SceneGraphLayer& other_layer) {
@@ -578,7 +579,7 @@ bool DynamicSceneGraph::mergeGraph(const DynamicSceneGraph& other,
     }
 
     std::vector<NodeId> new_nodes;
-    layer.mergeLayer(other_layer, config, &new_nodes);
+    layer.mergeLayer(other_layer, config, &new_nodes, transform_new_nodes);
     for (const auto node_id : new_nodes) {
       node_lookup_[node_id] = layer_key;
     }

src/scene_graph_layer.cpp

@@ -239,7 +239,8 @@ bool SceneGraphLayer::rewireEdge(NodeId source,
 
 void SceneGraphLayer::mergeLayer(const SceneGraphLayer& other_layer,
                                  const GraphMergeConfig& config,
-                                 std::vector<NodeId>* new_nodes) {
+                                 std::vector<NodeId>* new_nodes,
+                                 const Eigen::Affine3d* transform_new_nodes) {
   const bool update_attributes = config.shouldUpdateAttributes(id);
   for (const auto& id_node_pair : other_layer.nodes_) {
     const auto siter = nodes_status_.find(id_node_pair.first);
@@ -263,6 +264,9 @@ void SceneGraphLayer::mergeLayer(const SceneGraphLayer& other_layer,
     }
 
     auto attrs = other.attributes_->clone();
+    if (transform_new_nodes) {
+      attrs->position = *transform_new_nodes * attrs->position;
+    }
     nodes_[other.id] = Node::Ptr(new Node(other.id, id, std::move(attrs)));
     nodes_status_[other.id] = NodeStatus::NEW;
     if (new_nodes) {

tests/utest_scene_graph_layer.cpp

@@ -340,6 +340,44 @@ TEST(SceneGraphLayerTests, MergeLayerCorrect) {
   }
 }
 
+TEST(SceneGraphLayerTests, MergeLayerTransformCorrect) {
+  SceneGraphLayer layer_1(1);
+  SceneGraphLayer layer_2(1);
+
+  for (size_t i = 0; i < 3; ++i) {
+    Eigen::Vector3d node_pos;
+    node_pos << static_cast<double>(i), 0.0, 0.0;
+    EXPECT_TRUE(layer_1.emplaceNode(i, std::make_unique<NodeAttributes>(node_pos)));
+    layer_1.getNode(i).attributes().is_active = true;
+  }
+
+  for (size_t i = 0; i < 5; ++i) {
+    Eigen::Vector3d node_pos;
+    node_pos << static_cast<double>(i + 10), 0.0, 0.0;
+    EXPECT_TRUE(layer_2.emplaceNode(i, std::make_unique<NodeAttributes>(node_pos)));
+  }
+  Eigen::Matrix4d transform_matrix;
+  transform_matrix << -1, 0, 0, 0, 0, -1, 0, 2, 0, 0, 1, 3, 0, 0, 0, 1;
+
+  Eigen::Affine3d transform(transform_matrix);
+  std::vector<NodeId> new_nodes;
+  layer_1.mergeLayer(layer_2, {}, &new_nodes, &transform);
+
+  for (size_t i = 0; i < 3; i++) {
+    Eigen::Vector3d result = layer_1.getNode(i).attributes().position;
+    EXPECT_NEAR(static_cast<double>(i) + 10, result(0), 1.0e-9);
+    EXPECT_NEAR(0.0, result(1), 1.0e-9);
+    EXPECT_NEAR(0.0, result(2), 1.0e-9);
+  }
+
+  for (size_t i = 3; i < 5; i++) {
+    Eigen::Vector3d result = layer_1.getNode(i).attributes().position;
+    EXPECT_NEAR(-static_cast<double>(i) - 10, result(0), 1.0e-9);
+    EXPECT_NEAR(2.0, result(1), 1.0e-9);
+    EXPECT_NEAR(3.0, result(2), 1.0e-9);
+  }
+}
+
 TEST(SceneGraphLayerTests, GetNeighborhoodCorrect) {
   SceneGraphLayer layer(1);