add transform to merge graph

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

Hide file tree

Showing 6 changed files with 36 additions and 10 deletions.
diff --git a/include/spark_dsg/dynamic_scene_graph.h b/include/spark_dsg/dynamic_scene_graph.h
@@ -458,9 +458,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

diff --git a/include/spark_dsg/dynamic_scene_graph_layer.h b/include/spark_dsg/dynamic_scene_graph_layer.h
@@ -33,6 +33,8 @@
  * purposes notwithstanding any copyright notation herein.
  * -------------------------------------------------------------------------- */
 #pragma once
+#include <Eigen/Geometry>
+
 #include "spark_dsg/base_layer.h"
 #include "spark_dsg/edge_container.h"
 #include "spark_dsg/layer_prefix.h"
@@ -94,7 +96,8 @@ class DynamicSceneGraphLayer : public BaseLayer {
 
   void mergeLayer(const DynamicSceneGraphLayer& other,
                   const GraphMergeConfig& config,
-                  std::vector<NodeId>* new_nodes = nullptr);
+                  std::vector<NodeId>* new_nodes = nullptr,
+                  const Eigen::Affine3d* transform = 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
@@ -33,6 +33,7 @@
  * purposes notwithstanding any copyright notation herein.
  * -------------------------------------------------------------------------- */
 #pragma once
+#include <Eigen/Geometry>
 #include <functional>
 #include <map>
 #include <unordered_set>
@@ -157,7 +158,8 @@ class SceneGraphLayer : public BaseLayer {
 
   void mergeLayer(const SceneGraphLayer& other,
                   const GraphMergeConfig& config,
-                  std::vector<NodeId>* new_nodes);
+                  std::vector<NodeId>* new_nodes = nullptr,
+                  const Eigen::Affine3d* transform = nullptr);
 
   /**
    * @brief Number of nodes in the layer

diff --git a/src/dynamic_scene_graph.cpp b/src/dynamic_scene_graph.cpp
@@ -667,7 +667,8 @@ bool DynamicSceneGraph::updateFromLayer(SceneGraphLayer& other_layer,
 }
 
 bool DynamicSceneGraph::mergeGraph(const DynamicSceneGraph& other,
-                                   const GraphMergeConfig& config) {
+                                   const GraphMergeConfig& config,
+                                   const Eigen::Affine3d* transform) {
   for (auto&& [l_id, other_layers] : other.dynamicLayers()) {
     for (auto&& [prefix, other_layer] : other_layers) {
       if (!hasLayer(l_id, prefix)) {
@@ -676,7 +677,8 @@ 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);
+      dynamic_layers_[l_id][prefix]->mergeLayer(
+          *other_layer, config, &new_nodes, transform);
       for (const auto node_id : new_nodes) {
         node_lookup_[node_id] = layer_key;
       }
@@ -701,7 +703,7 @@ bool DynamicSceneGraph::mergeGraph(const DynamicSceneGraph& other,
     }
 
     std::vector<NodeId> new_nodes;
-    layers_[l_id]->mergeLayer(*other_layer, config, &new_nodes);
+    layers_[l_id]->mergeLayer(*other_layer, config, &new_nodes, transform);
     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,14 +49,18 @@ DynamicSceneGraphLayer::DynamicSceneGraphLayer(LayerId layer, LayerPrefix node_p
 
 void DynamicSceneGraphLayer::mergeLayer(const DynamicSceneGraphLayer& other,
                                         const GraphMergeConfig& config,
-                                        std::vector<NodeId>* new_nodes) {
+                                        std::vector<NodeId>* new_nodes,
+                                        const Eigen::Affine3d* transform) {
   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
-      const Eigen::Vector3d node_position = nodes_[i]->attributes_->position;
+      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) {
@@ -68,7 +72,12 @@ void DynamicSceneGraphLayer::mergeLayer(const DynamicSceneGraphLayer& other,
       nodes_[i]->attributes_->position = node_position;
     } else {
       emplaceNode(other_node.timestamp.value(), other_node.attributes_->clone(), false);
-      nodes_.back()->attributes_->position += last_update_delta;
+      if (transform) {
+        nodes_.back()->attributes_->position =
+            *transform * nodes_.back()->attributes_->position;
+      } else {
+        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
@@ -216,7 +216,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) {
   const bool update_attributes =
       (config.update_layer_attributes && config.update_layer_attributes->count(id))
           ? config.update_layer_attributes->at(id)
@@ -240,10 +241,16 @@ 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;
+    }
     nodes_[other.id] = Node::Ptr(new Node(other.id, id, std::move(attrs)));
     nodes_status_[other.id] = NodeStatus::NEW;
     if (new_nodes) {