(WIP) iterating on interface and purpose

MIT-SPARK · Feb 5, 2025 · eada154 · eada154
1 parent 8ab00e0
commit eada154
Show file tree

Hide file tree

Showing 6 changed files with 48 additions and 24 deletions.
diff --git a/include/spark_dsg/dynamic_scene_graph_layer.h b/include/spark_dsg/dynamic_scene_graph_layer.h
@@ -96,8 +96,7 @@ class DynamicSceneGraphLayer : public BaseLayer {
 
   void mergeLayer(const DynamicSceneGraphLayer& other,
                   const GraphMergeConfig& config,
-                  std::vector<NodeId>* new_nodes = nullptr,
-                  const Eigen::Affine3d* transform = nullptr);
+                  std::vector<NodeId>* new_nodes = nullptr);
 
   void getNewNodes(std::vector<NodeId>& new_nodes, bool clear_new) override;
 

diff --git a/include/spark_dsg/scene_graph_layer.h b/include/spark_dsg/scene_graph_layer.h
@@ -159,7 +159,7 @@ class SceneGraphLayer : public BaseLayer {
   void mergeLayer(const SceneGraphLayer& other,
                   const GraphMergeConfig& config,
                   std::vector<NodeId>* new_nodes = nullptr,
-                  const Eigen::Affine3d* transform = nullptr);
+                  const Eigen::Affine3d* transform_new_nodes = nullptr);
 
   /**
    * @brief Number of nodes in the layer

diff --git a/src/dynamic_scene_graph.cpp b/src/dynamic_scene_graph.cpp
@@ -668,7 +668,7 @@ bool DynamicSceneGraph::updateFromLayer(SceneGraphLayer& other_layer,
 
 bool DynamicSceneGraph::mergeGraph(const DynamicSceneGraph& other,
                                    const GraphMergeConfig& config,
-                                   const Eigen::Affine3d* transform) {
+                                   const Eigen::Affine3d* transform_new_nodes) {
   for (auto&& [l_id, other_layers] : other.dynamicLayers()) {
     for (auto&& [prefix, other_layer] : other_layers) {
       if (!hasLayer(l_id, prefix)) {
@@ -677,8 +677,7 @@ bool DynamicSceneGraph::mergeGraph(const DynamicSceneGraph& other,
 
       const LayerKey layer_key(l_id, prefix);
       std::vector<NodeId> new_nodes;
-      dynamic_layers_[l_id][prefix]->mergeLayer(
-          *other_layer, config, &new_nodes, transform);
+      dynamic_layers_[l_id][prefix]->mergeLayer(*other_layer, config, &new_nodes);
       for (const auto node_id : new_nodes) {
         node_lookup_[node_id] = layer_key;
       }
@@ -703,7 +702,7 @@ bool DynamicSceneGraph::mergeGraph(const DynamicSceneGraph& other,
     }
 
     std::vector<NodeId> new_nodes;
-    layers_[l_id]->mergeLayer(*other_layer, config, &new_nodes, transform);
+    layers_[l_id]->mergeLayer(*other_layer, config, &new_nodes, transform_new_nodes);
     for (const auto node_id : new_nodes) {
       node_lookup_[node_id] = l_id;
     }

diff --git a/src/dynamic_scene_graph_layer.cpp b/src/dynamic_scene_graph_layer.cpp
@@ -49,18 +49,14 @@ DynamicSceneGraphLayer::DynamicSceneGraphLayer(LayerId layer, LayerPrefix node_p
 
 void DynamicSceneGraphLayer::mergeLayer(const DynamicSceneGraphLayer& other,
                                         const GraphMergeConfig& config,
-                                        std::vector<NodeId>* new_nodes,
-                                        const Eigen::Affine3d* transform) {
+                                        std::vector<NodeId>* new_nodes) {
   Eigen::Vector3d last_update_delta = Eigen::Vector3d::Zero();
 
   for (size_t i = 0; i < other.nodes_.size(); i++) {
     const auto& other_node = *other.nodes_[i];
     if (i < next_node_) {
       // update the last_update_delta
       Eigen::Vector3d node_position = nodes_[i]->attributes_->position;
-      if (transform) {
-        node_position = *transform * node_position;
-      }
       last_update_delta = node_position - other_node.attributes_->position;
 
       if (!config.update_dynamic_attributes) {
@@ -72,12 +68,7 @@ void DynamicSceneGraphLayer::mergeLayer(const DynamicSceneGraphLayer& other,
       nodes_[i]->attributes_->position = node_position;
     } else {
       emplaceNode(other_node.timestamp.value(), other_node.attributes_->clone(), false);
-      if (transform) {
-        nodes_.back()->attributes_->position =
-            *transform * nodes_.back()->attributes_->position;
-      } else {
-        nodes_.back()->attributes_->position += last_update_delta;
-      }
+      nodes_.back()->attributes_->position += last_update_delta;
       if (new_nodes) {
         new_nodes->push_back(nodes_.back()->id);
       }

diff --git a/src/scene_graph_layer.cpp b/src/scene_graph_layer.cpp
@@ -217,7 +217,7 @@ bool SceneGraphLayer::rewireEdge(NodeId source,
 void SceneGraphLayer::mergeLayer(const SceneGraphLayer& other_layer,
                                  const GraphMergeConfig& config,
                                  std::vector<NodeId>* new_nodes,
-                                 const Eigen::Affine3d* transform) {
+                                 const Eigen::Affine3d* transform_new_nodes) {
   const bool update_attributes =
       (config.update_layer_attributes && config.update_layer_attributes->count(id))
           ? config.update_layer_attributes->at(id)
@@ -241,15 +241,12 @@ void SceneGraphLayer::mergeLayer(const SceneGraphLayer& other_layer,
       }
 
       iter->second->attributes_ = other.attributes_->clone();
-      if (transform) {
-        iter->second->attributes_->position = *transform * other.attributes_->position;
-      }
       continue;
     }
 
     auto attrs = other.attributes_->clone();
-    if (transform) {
-      attrs->position = *transform * attrs->position;
+    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;

diff --git a/tests/utest_scene_graph_layer.cpp b/tests/utest_scene_graph_layer.cpp
@@ -371,6 +371,44 @@ TEST(SceneGraphLayerTests, MergeLayerCorrect) {
   }
 }
 
+TEST(SceneGraphLayerTests, MergeLayerTransformCorrect) {
+  IsolatedSceneGraphLayer layer_1(1);
+  IsolatedSceneGraphLayer 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) {
   IsolatedSceneGraphLayer layer(1);