fix(backend): survive merge parents deleted from the unmerged graph

src/backend/merge_tracker.cpp

@@ -80,6 +80,14 @@ size_t MergeTracker::applyMerges(const DynamicSceneGraph& unmerged,
       continue;
     }
 
+    // the frontend can delete nodes, so the recorded parent may be gone from either
+    // graph; the merged attributes can never be rebuilt again in that case
+    if (!unmerged.hasNode(node) || !graph.hasNode(node)) {
+      VLOG(1) << "Dropping merge set for missing parent " << NodeSymbol(node).str();
+      merge_sets_.erase(iter);
+      continue;
+    }
+
     auto child_iter = iter->second.begin();
     while (child_iter != iter->second.end()) {
       if (!unmerged.hasNode(*child_iter)) {
@@ -91,7 +99,10 @@ size_t MergeTracker::applyMerges(const DynamicSceneGraph& unmerged,
 
     std::vector<NodeId> nodes{node};
     nodes.insert(nodes.end(), iter->second.begin(), iter->second.end());
-    graph.setNodeAttributes(node, merge_attrs(unmerged, nodes));
+    auto attrs = merge_attrs(unmerged, nodes);
+    if (attrs) {
+      graph.setNodeAttributes(node, std::move(attrs));
+    }
   }
 
   return num_applied;
@@ -100,7 +111,18 @@ size_t MergeTracker::applyMerges(const DynamicSceneGraph& unmerged,
 void MergeTracker::updateAllMergeAttributes(const DynamicSceneGraph& unmerged,
                                             DynamicSceneGraph& merged,
                                             const MergeFunc& merge_attrs) {
-  for (auto& [parent, children] : merge_sets_) {
+  auto iter = merge_sets_.begin();
+  while (iter != merge_sets_.end()) {
+    const auto parent = iter->first;
+    // the frontend can delete nodes, so the recorded parent may be gone from either
+    // graph; the merged attributes can never be rebuilt again in that case
+    if (!unmerged.hasNode(parent) || !merged.hasNode(parent)) {
+      VLOG(1) << "Dropping merge set for missing parent " << NodeSymbol(parent).str();
+      iter = merge_sets_.erase(iter);
+      continue;
+    }
+
+    auto& children = iter->second;
     auto child_iter = children.begin();
     while (child_iter != children.end()) {
       if (!unmerged.hasNode(*child_iter)) {
@@ -112,7 +134,11 @@ void MergeTracker::updateAllMergeAttributes(const DynamicSceneGraph& unmerged,
 
     std::vector<NodeId> nodes{parent};
     nodes.insert(nodes.end(), children.begin(), children.end());
-    merged.setNodeAttributes(parent, merge_attrs(unmerged, nodes));
+    auto attrs = merge_attrs(unmerged, nodes);
+    if (attrs) {
+      merged.setNodeAttributes(parent, std::move(attrs));
+    }
+    ++iter;
   }
 }
 

tests/CMakeLists.txt

@@ -12,6 +12,7 @@ add_executable(
   src/resources.cpp
   backend/test_external_loop_closure.cpp
   backend/test_generic_update_functor.cpp
+  backend/test_merge_tracker.cpp
   backend/test_update_agents_functor.cpp
   backend/test_update_buildings_functor.cpp
   backend/test_update_objects_functor.cpp

tests/backend/test_merge_tracker.cpp

@@ -0,0 +1,108 @@
+/* -----------------------------------------------------------------------------
+ * 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/backend/merge_tracker.h>
+#include <spark_dsg/node_attributes.h>
+#include <spark_dsg/node_symbol.h>
+
+#include "hydra_test/shared_dsg_fixture.h"
+
+namespace hydra {
+
+namespace {
+
+using spark_dsg::NodeSymbol;
+
+void addObject(DynamicSceneGraph& graph, size_t index) {
+  auto attrs = std::make_unique<spark_dsg::ObjectNodeAttributes>();
+  attrs->position << static_cast<double>(index), 0.0, 0.0;
+  graph.emplaceNode(DsgLayers::OBJECTS, NodeSymbol('O', index), std::move(attrs));
+}
+
+// simple merge hook that clones the parent's attributes from the unmerged graph,
+// mirroring what real merge hooks do
+MergeTracker::MergeFunc cloneParentAttrs() {
+  return [](const DynamicSceneGraph& unmerged, const std::vector<NodeId>& nodes) {
+    const auto node = unmerged.findNode(nodes.front());
+    return node ? node->attributes().clone() : nullptr;
+  };
+}
+
+}  // namespace
+
+// The frontend can delete nodes, so a merge parent recorded by the tracker may
+// vanish from the unmerged graph. Both entry points must drop that merge set
+// instead of throwing on the missing node.
+TEST(MergeTracker, DeletedParentDoesNotThrow) {
+  auto dsg = test::makeSharedDsg();
+  auto& merged = *dsg->graph;
+  addObject(merged, 0);
+  addObject(merged, 1);
+  const auto unmerged = merged.clone();
+
+  MergeTracker tracker;
+  const auto merge_attrs = cloneParentAttrs();
+  MergeList proposals{{NodeSymbol('O', 1), NodeSymbol('O', 0)}};
+  ASSERT_EQ(tracker.applyMerges(*unmerged, proposals, *dsg, merge_attrs), 1u);
+
+  // frontend deletes the surviving node from the unmerged graph
+  unmerged->removeNode(NodeSymbol('O', 0));
+
+  EXPECT_NO_THROW(tracker.updateAllMergeAttributes(*unmerged, merged, merge_attrs));
+  MergeList repeat{{NodeSymbol('O', 1), NodeSymbol('O', 0)}};
+  EXPECT_NO_THROW(tracker.applyMerges(*unmerged, repeat, *dsg, merge_attrs));
+}
+
+// A merge hook may return nullptr (e.g., when it cannot rebuild attributes);
+// the tracker must not install null attributes on the merged node.
+TEST(MergeTracker, NullMergeAttrsSkipped) {
+  auto dsg = test::makeSharedDsg();
+  auto& merged = *dsg->graph;
+  addObject(merged, 0);
+  addObject(merged, 1);
+  const auto unmerged = merged.clone();
+
+  MergeTracker tracker;
+  const MergeTracker::MergeFunc null_attrs = [](const DynamicSceneGraph&,
+                                                const std::vector<NodeId>&) {
+    return NodeAttributes::Ptr(nullptr);
+  };
+  MergeList proposals{{NodeSymbol('O', 1), NodeSymbol('O', 0)}};
+  EXPECT_EQ(tracker.applyMerges(*unmerged, proposals, *dsg, null_attrs), 1u);
+  // the surviving node keeps valid attributes
+  EXPECT_NO_THROW(merged.getNode(NodeSymbol('O', 0)).attributes());
+}
+
+}  // namespace hydra