diff --git a/src/backend/merge_tracker.cpp b/src/backend/merge_tracker.cpp index 4f67880b..592353d5 100644 --- a/src/backend/merge_tracker.cpp +++ b/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 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 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; } } diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 67315da1..00ea364d 100644 --- a/tests/CMakeLists.txt +++ b/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 diff --git a/tests/backend/test_merge_tracker.cpp b/tests/backend/test_merge_tracker.cpp new file mode 100644 index 00000000..5ba69302 --- /dev/null +++ b/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 +#include +#include +#include + +#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(); + attrs->position << static_cast(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& 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&) { + 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