feat(common): archive tracked nodes absent from an update round (+ propagate archived attrs to the backend)

include/hydra/common/graph_update.h

@@ -76,6 +76,8 @@ struct LayerTracker {
     char prefix = 0;
     std::optional<spark_dsg::LayerId> target_layer;
     config::VirtualConfig<NodeMatcher> matcher;
+    //! @brief archive (is_active=false) tracked nodes absent from an update round
+    bool archive_missing = false;
   } const config;
 
   explicit LayerTracker(const Config& config);

src/backend/backend_module.cpp

@@ -442,7 +442,12 @@ bool BackendModule::updatePrivateDsg(size_t timestamp_ns, bool force_update) {
       return false;
     }
 
-    unmerged_graph_->mergeGraph(*shared_dsg.graph);
+    // keep the odometric mirror faithful even for archived nodes: late attribute
+    // updates (e.g. an extractor finalizing a node after it archives) must still
+    // reach the backend
+    GraphMergeConfig merge_config;
+    merge_config.update_archived_attributes = true;
+    unmerged_graph_->mergeGraph(*shared_dsg.graph, merge_config);
   }  // end joint critical section
 
   backend_graph_logger_.logGraph(*private_dsg_->graph);

src/common/graph_update.cpp

@@ -41,6 +41,7 @@
 #include <spark_dsg/dynamic_scene_graph.h>
 
 #include <algorithm>
+#include <unordered_set>
 
 namespace YAML {
 
@@ -82,6 +83,7 @@ void declare_config(LayerTracker::Config& config) {
   field(config.target_layer, "target_layer");
   config.matcher.setOptional();
   field(config.matcher, "matcher");
+  field(config.archive_missing, "archive_missing");
 }
 
 void declare_config(GraphUpdater::Config& config) {
@@ -225,7 +227,11 @@ void GraphUpdater::update(const GraphUpdate& update, DynamicSceneGraph& graph) {
     const auto target_layer_id = tracker.config.target_layer.value_or(layer_id);
 
     std::map<NodeId, const NodeAttributes*> active_targets;
+    std::unordered_set<size_t> seen_tracks;
     for (auto&& entry : layer_update->updates) {
+      if (entry.track_id) {
+        seen_tracks.insert(*entry.track_id);
+      }
       switch (entry.update_type) {
         case NodeUpdate::UpdateType::Delete: {
           deleteNode(entry, tracker, graph);
@@ -243,6 +249,20 @@ void GraphUpdater::update(const GraphUpdate& update, DynamicSceneGraph& graph) {
         }
       }
     }
+
+    if (tracker.config.archive_missing) {
+      // tracks the active window stopped emitting have left the window; archiving
+      // them makes the node visible to archived-only merge candidates (Pairwise)
+      for (const auto& [track, node_id] : tracker.track_to_node) {
+        if (seen_tracks.count(track)) {
+          continue;
+        }
+        const auto node = graph.findNode(node_id);
+        if (node && node->attributes().is_active) {
+          node->attributes().is_active = false;
+        }
+      }
+    }
   }
 }
 

src/frontend/graph_builder.cpp

@@ -346,7 +346,11 @@ void GraphBuilder::spinOnce(const ActiveWindowOutput::Ptr& msg) {
     std::unique_lock<std::mutex> lock(state_->backend_graph->mutex);
     ScopedTimer merge_timer("frontend/merge_graph", msg->timestamp_ns);
     state_->backend_graph->sequence_number = sequence_number_;
-    state_->backend_graph->graph->mergeGraph(*dsg_->graph);
+    // archived nodes still receive attribute updates (e.g. the object extractor's
+    // image_folder lands after the track archives) that the backend must see
+    GraphMergeConfig merge_config;
+    merge_config.update_archived_attributes = true;
+    state_->backend_graph->graph->mergeGraph(*dsg_->graph, merge_config);
   }  // end critical section
 
   if (queues.lcd_queue) {

tests/CMakeLists.txt

@@ -17,6 +17,7 @@ add_executable(
   backend/test_update_objects_functor.cpp
   backend/test_update_places_functor.cpp
   common/test_config_utilities.cpp
+  common/test_graph_update.cpp
   common/test_shared_dsg_info.cpp
   frontend/test_graph_connector.cpp
   frontend/test_mesh_segmenter.cpp

tests/common/test_graph_update.cpp

@@ -0,0 +1,119 @@
+/* -----------------------------------------------------------------------------
+ * Copyright 2022 Massachusetts Institute of Technology.
+ * All Rights Reserved
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *  1. Redistributions of source code must retain the above copyright notice,
+ *     this list of conditions and the following disclaimer.
+ *
+ *  2. Redistributions in binary form must reproduce the above copyright notice,
+ *     this list of conditions and the following disclaimer in the documentation
+ *     and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Research was sponsored by the United States Air Force Research Laboratory and
+ * the United States Air Force Artificial Intelligence Accelerator and was
+ * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views
+ * and conclusions contained in this document are those of the authors and should
+ * not be interpreted as representing the official policies, either expressed or
+ * implied, of the United States Air Force or the U.S. Government. The U.S.
+ * Government is authorized to reproduce and distribute reprints for Government
+ * purposes notwithstanding any copyright notation herein.
+ * -------------------------------------------------------------------------- */
+#include <gtest/gtest.h>
+#include <hydra/common/graph_update.h>
+#include <spark_dsg/node_attributes.h>
+
+#include "hydra_test/shared_dsg_fixture.h"
+
+namespace hydra {
+
+namespace {
+
+GraphUpdater::Config makeConfig(bool archive_missing) {
+  GraphUpdater::Config config;
+  auto& tracker = config.layer_updates["OBJECTS"];
+  tracker.prefix = 'O';
+  tracker.archive_missing = archive_missing;
+  return config;
+}
+
+LayerUpdate::Ptr makeUpdate(spark_dsg::LayerId layer,
+                            const std::vector<size_t>& track_ids) {
+  auto update = std::make_shared<LayerUpdate>(layer);
+  for (const auto track : track_ids) {
+    auto attrs = std::make_shared<spark_dsg::KhronosObjectAttributes>();
+    attrs->position.setZero();
+    update->updates.push_back(NodeUpdate{attrs, track});
+  }
+  return update;
+}
+
+}  // namespace
+
+// A track present in one round and absent in the next gets archived; a track that
+// reappears is re-activated by the normal update path.
+TEST(GraphUpdater, ArchiveMissingTracks) {
+  auto dsg = test::makeSharedDsg();
+  auto& graph = *dsg->graph;
+  const auto layer_id = graph.getLayerKey("OBJECTS")->layer;
+
+  GraphUpdater updater(makeConfig(true));
+
+  GraphUpdate round1;
+  round1[layer_id] = makeUpdate(layer_id, {7, 8});
+  updater.update(round1, graph);
+
+  const spark_dsg::NodeSymbol n0('O', 0);
+  const spark_dsg::NodeSymbol n1('O', 1);
+  ASSERT_TRUE(graph.hasNode(n0));
+  ASSERT_TRUE(graph.hasNode(n1));
+  EXPECT_TRUE(graph.getNode(n0).attributes().is_active);
+  EXPECT_TRUE(graph.getNode(n1).attributes().is_active);
+
+  // round 2: track 8 vanished (left the active window)
+  GraphUpdate round2;
+  round2[layer_id] = makeUpdate(layer_id, {7});
+  updater.update(round2, graph);
+  EXPECT_TRUE(graph.getNode(n0).attributes().is_active);
+  EXPECT_FALSE(graph.getNode(n1).attributes().is_active);
+
+  // round 3: track 8 comes back -> normal update path re-activates it
+  GraphUpdate round3;
+  round3[layer_id] = makeUpdate(layer_id, {7, 8});
+  updater.update(round3, graph);
+  EXPECT_TRUE(graph.getNode(n1).attributes().is_active);
+}
+
+// Default (archive_missing=false) preserves the old always-active behavior.
+TEST(GraphUpdater, NoArchivalByDefault) {
+  auto dsg = test::makeSharedDsg();
+  auto& graph = *dsg->graph;
+  const auto layer_id = graph.getLayerKey("OBJECTS")->layer;
+
+  GraphUpdater updater(makeConfig(false));
+
+  GraphUpdate round1;
+  round1[layer_id] = makeUpdate(layer_id, {7, 8});
+  updater.update(round1, graph);
+
+  GraphUpdate round2;
+  round2[layer_id] = makeUpdate(layer_id, {7});
+  updater.update(round2, graph);
+
+  EXPECT_TRUE(graph.getNode(spark_dsg::NodeSymbol('O', 1)).attributes().is_active);
+}
+
+}  // namespace hydra