From 0739d9c9f59115325f42be1161ad4d5056ada91b Mon Sep 17 00:00:00 2001 From: harelb Date: Wed, 8 Jul 2026 10:38:58 -0400 Subject: [PATCH 1/7] perf(backend): full PGO solve only on new loop closures have_loopclosures_ is latched forever, so after the first LC every spin ran a full KimeraRpgo batch solve (1.86s x 314 spins for ONE LC in the box_7 run = 41% of backend time), starving the queue and lagging the published DSG by 15-20 min. Solve only when new LC factors arrived; between solves deform with the cached optimizer values (temp values cover not-yet-solved nodes). --- src/backend/backend_module.cpp | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/src/backend/backend_module.cpp b/src/backend/backend_module.cpp index bce80d9e..43c301f7 100644 --- a/src/backend/backend_module.cpp +++ b/src/backend/backend_module.cpp @@ -260,11 +260,25 @@ bool BackendModule::spinOnce(bool force_update) { } timer.reset("backend/spin"); - if ((config.optimize_on_lc && have_loopclosures_) || force_optimize_) { + if ((config.optimize_on_lc && have_new_loopclosures_) || force_optimize_) { optimize(timestamp_ns); } else { updateDsgMesh(timestamp_ns); - UpdateInfo::ConstPtr info(new UpdateInfo{timestamp_ns}); + UpdateInfo::ConstPtr info; + if (have_loopclosures_) { + // no new factors to solve: deform new/active nodes and mesh with the cached + // optimized values; the full solve only runs when new loop closures arrive + info.reset(new UpdateInfo{timestamp_ns, + deformation_graph_->getTempValues(), + deformation_graph_->getValues(), + false, + {}, + deformation_graph_.get(), + nullptr, + mesh_offsets_}); + } else { + info.reset(new UpdateInfo{timestamp_ns}); + } dsg_updater_->callUpdateFunctions(timestamp_ns, info); } From ffb51b67b0c1eb0d851101051fe63129908544e2 Mon Sep 17 00:00:00 2001 From: harelb Date: Wed, 8 Jul 2026 10:40:03 -0400 Subject: [PATCH 2/7] fix(backend): logStatus reads the timers that actually exist backend/optimization and backend/mesh_update were never registered, so dsg_pgmo_status.csv optimize_time/mesh_update_time were always NaN. --- src/backend/backend_module.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/backend/backend_module.cpp b/src/backend/backend_module.cpp index 43c301f7..586d2c49 100644 --- a/src/backend/backend_module.cpp +++ b/src/backend/backend_module.cpp @@ -569,8 +569,8 @@ void BackendModule::logStatus() { auto& status = status_log_.back(); const auto& timer = hydra::timing::ElapsedTimeRecorder::instance(); status.last_spin_s = timer.getLastElapsed("backend/spin"); - status.last_opt_s = timer.getLastElapsed("backend/optimization"); - status.last_mesh_update_s = timer.getLastElapsed("backend/mesh_update"); + status.last_opt_s = timer.getLastElapsed("dsg_updater/optimization"); + status.last_mesh_update_s = timer.getLastElapsed("backend/mesh_deformation"); } } // namespace hydra From d49f46b3aa267035831ca42a613c7cc260c1c9cc Mon Sep 17 00:00:00 2001 From: harelb Date: Tue, 7 Jul 2026 23:05:57 -0400 Subject: [PATCH 3/7] fix(backend): hoist per-spin bookkeeping out of the update-functor loop The cleanup-hook invocation, the forced-active flag restore, and the new-node status clear all ran inside the per-functor loop, so only the first functor of each spin observed forced-active nodes and new-node status, and earlier functors' cleanup hooks re-ran once per remaining functor. Move all three after the loop so they happen once per spin. --- src/backend/dsg_updater.cpp | 30 +++--- tests/CMakeLists.txt | 1 + tests/backend/test_dsg_updater.cpp | 143 +++++++++++++++++++++++++++++ 3 files changed, 159 insertions(+), 15 deletions(-) create mode 100644 tests/backend/test_dsg_updater.cpp diff --git a/src/backend/dsg_updater.cpp b/src/backend/dsg_updater.cpp index 2e721a17..9899c3a5 100644 --- a/src/backend/dsg_updater.cpp +++ b/src/backend/dsg_updater.cpp @@ -213,25 +213,25 @@ void DsgUpdater::callUpdateFunctions(size_t timestamp_ns, UpdateInfo::ConstPtr i *source_graph_, *target_dsg_->graph, hooks.merge); } } + } - // TODO(nathan) fix args for multithreading - MLOG(2) << "all merges: " << merge_tracker.print(); - for (const auto& func : cleanup_hooks) { - func(info, *source_graph_, target_dsg_.get()); - } + // TODO(nathan) fix args for multithreading + MLOG(2) << "all merges: " << merge_tracker.print(); + for (const auto& func : cleanup_hooks) { + func(info, *source_graph_, target_dsg_.get()); + } - // We reset active flags for all new nodes that were inactive after one update - for (auto node_id : active_nodes_to_restore) { - auto node = source_graph_->findNode(node_id); - if (node) { - node->attributes().is_active = false; - } + // We reset active flags for all new nodes that were inactive after one update + for (auto node_id : active_nodes_to_restore) { + auto node = source_graph_->findNode(node_id); + if (node) { + node->attributes().is_active = false; } - - // clear new node status - // TODO(nathan) add API for marking new nodes - source_graph_->getNewNodes(true); } + + // clear new node status + // TODO(nathan) add API for marking new nodes + source_graph_->getNewNodes(true); } } // namespace hydra diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 67315da1..7c8b00a9 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -10,6 +10,7 @@ add_executable( test_${PROJECT_NAME} main.cpp src/resources.cpp + backend/test_dsg_updater.cpp backend/test_external_loop_closure.cpp backend/test_generic_update_functor.cpp backend/test_update_agents_functor.cpp diff --git a/tests/backend/test_dsg_updater.cpp b/tests/backend/test_dsg_updater.cpp new file mode 100644 index 00000000..d609821b --- /dev/null +++ b/tests/backend/test_dsg_updater.cpp @@ -0,0 +1,143 @@ +/* ----------------------------------------------------------------------------- + * 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 { + +struct CallRecord { + std::string functor; + bool node_active; + bool node_new; +}; + +std::vector& callRecords() { + static std::vector records; + return records; +} + +std::map& cleanupCounts() { + static std::map counts; + return counts; +} + +NodeId trackedNode() { return spark_dsg::NodeSymbol('p', 0); } + +// Records, per functor invocation, how the shared per-spin bookkeeping (forced-active +// flags and new-node status on the source graph) looks from inside the functor. +struct RecordingFunctor : UpdateFunctor { + struct Config { + std::string name; + } const config; + + explicit RecordingFunctor(const Config& config) : config(config) {} + + Hooks hooks() const override { + auto my_hooks = UpdateFunctor::hooks(); + my_hooks.cleanup = [name = config.name](const UpdateInfo::ConstPtr&, + DynamicSceneGraph&, + SharedDsgInfo*) { + ++cleanupCounts()[name]; + }; + return my_hooks; + } + + void call(const DynamicSceneGraph& unmerged, + SharedDsgInfo&, + const UpdateInfo::ConstPtr&) const override { + const auto& attrs = unmerged.getNode(trackedNode()).attributes(); + callRecords().push_back({config.name, + attrs.is_active, + unmerged.checkNode(trackedNode()) == NodeStatus::NEW}); + } + + inline static const auto registration_ = + config::RegistrationWithConfig( + "RecordingFunctor"); +}; + +void declare_config(RecordingFunctor::Config& config) { + config::name("RecordingFunctor::Config"); + config::field(config.name, "name"); +} + +} // namespace + +TEST(DsgUpdater, PerSpinBookkeepingSharedAcrossFunctors) { + callRecords().clear(); + cleanupCounts().clear(); + + auto target = test::makeSharedDsg(); + DynamicSceneGraph::Ptr source = target->graph->clone(); + + // a NEW node that archived before the backend ever saw it + auto attrs = std::make_unique(); + attrs->is_active = false; + source->emplaceNode(DsgLayers::PLACES, trackedNode(), std::move(attrs)); + + DsgUpdater::Config config; + config.update_functors.emplace_back( + "a", DsgUpdater::Config::FunctorConfig(RecordingFunctor::Config{"a"})); + config.update_functors.emplace_back( + "b", DsgUpdater::Config::FunctorConfig(RecordingFunctor::Config{"b"})); + + DsgUpdater updater(config, source, target); + UpdateInfo::ConstPtr info(new UpdateInfo{0}); + updater.callUpdateFunctions(0, info); + + // every functor in the spin should observe the forced-active flag and the new-node + // status; both are per-spin state that must only reset after all functors ran + ASSERT_EQ(callRecords().size(), 2u); + EXPECT_EQ(callRecords()[0].functor, "a"); + EXPECT_TRUE(callRecords()[0].node_active); + EXPECT_TRUE(callRecords()[0].node_new); + EXPECT_EQ(callRecords()[1].functor, "b"); + EXPECT_TRUE(callRecords()[1].node_active); + EXPECT_TRUE(callRecords()[1].node_new); + + // each cleanup hook runs exactly once per spin + EXPECT_EQ(cleanupCounts()["a"], 1u); + EXPECT_EQ(cleanupCounts()["b"], 1u); + + // after the spin the forced-active flag is restored + EXPECT_FALSE(source->getNode(trackedNode()).attributes().is_active); +} + +} // namespace hydra From 0e0536bc4735279a3e4c17cde34dec275216c077 Mon Sep 17 00:00:00 2001 From: harelb Date: Tue, 7 Jul 2026 23:11:18 -0400 Subject: [PATCH 4/7] fix(backend): stop mutating unmerged places positions in updateFromValues updateFromValues wrote the optimized position through the unmerged layer view before cloning into the merged graph, leaking optimized state into the unmerged (odometric) graph. Clone first and only write the merged graph, and find place merges on the merged graph like the object tests already do. --- src/backend/update_places_functor.cpp | 8 ++-- tests/backend/test_update_places_functor.cpp | 39 +++++++++++++++++++- 2 files changed, 43 insertions(+), 4 deletions(-) diff --git a/src/backend/update_places_functor.cpp b/src/backend/update_places_functor.cpp index cc508298..29b1eba6 100644 --- a/src/backend/update_places_functor.cpp +++ b/src/backend/update_places_functor.cpp @@ -116,9 +116,11 @@ size_t UpdatePlacesFunctor::updateFromValues(const LayerView& view, // TODO(nathan) consider updating distance via parents + deformation graph ++num_changed; - auto& attrs = node.attributes(); - attrs.position = places_values.at(node.id).translation(); - dsg.graph->setNodeAttributes(node.id, attrs.clone()); + // write the optimized position into the merged graph only; the unmerged view + // stays odometric + auto new_attrs = node.attributes().clone(); + new_attrs->position = places_values.at(node.id).translation(); + dsg.graph->setNodeAttributes(node.id, std::move(new_attrs)); } // TODO(nathan) fix this diff --git a/tests/backend/test_update_places_functor.cpp b/tests/backend/test_update_places_functor.cpp index ed00bd85..6110ebfc 100644 --- a/tests/backend/test_update_places_functor.cpp +++ b/tests/backend/test_update_places_functor.cpp @@ -50,7 +50,9 @@ MergeList callWithUnmerged(const UpdateFunctor& functor, functor.call(*unmerged, dsg, info); const auto hooks = functor.hooks(); if (enable_merging && hooks.find_merges) { - return hooks.find_merges(*unmerged, info); + // mirror DsgUpdater: merges are found on the merged graph, which carries the + // updated (optimized) geometry; the unmerged graph stays odometric + return hooks.find_merges(*dsg.graph, info); } else { return {}; } @@ -107,6 +109,41 @@ TEST(UpdatePlacesFunctor, PlaceUpdate) { } } +TEST(UpdatePlacesFunctor, UpdateFromValuesLeavesUnmergedUntouched) { + auto dsg = test::makeSharedDsg(); + auto& graph = *dsg->graph; + + auto attrs = std::make_unique(0.0, 0.0); + attrs->position = Eigen::Vector3d(1.0, 2.0, 3.0); + attrs->is_active = true; + graph.emplaceNode(DsgLayers::PLACES, NodeSymbol('p', 0), std::move(attrs)); + + const auto unmerged = graph.clone(); + + gtsam::Values values; + values.insert(NodeSymbol('p', 0), + gtsam::Pose3(gtsam::Rot3(), gtsam::Point3(4.0, 5.0, 6.0))); + + UpdateInfo::ConstPtr info(new UpdateInfo{0, &values, nullptr, true, {}}); + UpdatePlacesFunctor::Config config{0.4, 0.3, DeformationInterpolator::Config{}}; + config.use_temp_values = true; + + const UpdatePlacesFunctor functor(config); + functor.call(*unmerged, *dsg, info); + + { // the optimized value lands in the merged graph + Eigen::Vector3d expected(4.0, 5.0, 6.0); + Eigen::Vector3d result = graph.getPosition(NodeSymbol('p', 0)); + EXPECT_NEAR(0.0, (result - expected).norm(), 1.0e-7); + } + + { // the unmerged (odometric) graph keeps its original position + Eigen::Vector3d expected(1.0, 2.0, 3.0); + Eigen::Vector3d result = unmerged->getPosition(NodeSymbol('p', 0)); + EXPECT_NEAR(0.0, (result - expected).norm(), 1.0e-7); + } +} + TEST(UpdatePlacesFunctor, PlaceUpdateNodeFinderBug) { auto dsg = test::makeSharedDsg(); auto& graph = *dsg->graph; From de49dc7fdf07a3f4a48e1fab86beb08a9f2049ce Mon Sep 17 00:00:00 2001 From: harelb Date: Wed, 8 Jul 2026 16:32:17 -0400 Subject: [PATCH 5/7] refactor(backend): keep unmerged graph odometric; derive merged poses via per-node transforms Partially supersedes the narrow init_pos cache: with the in-place unmerged position write removed, the unmerged graph is odometric by construction and IS the source of original values, so the cache no longer needs to freeze per-node state. - the deform callback no longer writes into the unmerged graph; it reads the odometric source attributes and writes position, bounding box, and (new) world_R_object field-wise into the merged graph only, via applyNodeDeformation (field-wise so merged-only state, e.g. merge-accumulated mesh connections, survives) - NodeCache drops init_pos freezing, refreshes unconditionally, and records each node's last odometric->optimized transform - the traversability merge hook re-applies the surviving node's last transform to attributes rebuilt from odometric unmerged constituents, keeping merged parents in the optimized frame after applyMerges/updateAllMergeAttributes - documents the odometric invariant on unmerged_graph_/source_graph_ (relies on every post-LC spin receiving optimizer values - fresh from a solve or cached, per the new-loop-closure gating - so merged active nodes reset by mergeGraph are re-derived) --- include/hydra/backend/backend_module.h | 6 + .../hydra/backend/deformation_interpolator.h | 57 +++-- include/hydra/backend/dsg_updater.h | 2 + src/backend/deformation_interpolator.cpp | 73 +++++-- ..._region_growing_traversability_functor.cpp | 9 +- tests/CMakeLists.txt | 1 + .../backend/test_deformation_interpolator.cpp | 194 ++++++++++++++++++ tests/backend/test_generic_update_functor.cpp | 16 +- 8 files changed, 326 insertions(+), 32 deletions(-) create mode 100644 tests/backend/test_deformation_interpolator.cpp diff --git a/include/hydra/backend/backend_module.h b/include/hydra/backend/backend_module.h index f9f2e182..61cf4a19 100644 --- a/include/hydra/backend/backend_module.h +++ b/include/hydra/backend/backend_module.h @@ -162,6 +162,12 @@ class BackendModule : public kimera_pgmo::KimeraPgmoInterface, public Module { uint64_t last_sequence_number_ = 0; SharedDsgInfo::Ptr private_dsg_; + //! Unmerged copy of the scene graph holding ODOMETRIC (never optimized) poses. + //! The merged private_dsg_ is derived from it via per-node odometric->optimized + //! transforms every optimize spin (have_loopclosures_ latches, so post-LC every + //! spin re-optimizes and re-deforms the active merged nodes that mergeGraph reset + //! to odometric clones). Nodes the interpolator skips (control-point matching + //! failure) or layers without a deform functor stay odometric while active. DynamicSceneGraph::Ptr unmerged_graph_; SharedModuleState::Ptr state_; diff --git a/include/hydra/backend/deformation_interpolator.h b/include/hydra/backend/deformation_interpolator.h index adcd5e1c..e765f2f9 100644 --- a/include/hydra/backend/deformation_interpolator.h +++ b/include/hydra/backend/deformation_interpolator.h @@ -35,20 +35,47 @@ #pragma once #include +#include #include "hydra/backend/update_functions.h" namespace hydra { +/** + * @brief Write the deformed (optimized) version of the odometric source attributes + * into the destination attributes. + * + * Only spatial fields are written (position, bounding box, object orientation), so + * merged-only state on the destination survives. Everything is derived from the + * odometric source, never from the destination, so repeated application with the + * same transform is idempotent. + */ +void applyNodeDeformation(const Eigen::Isometry3d& transform, + const NodeAttributes& src, + NodeAttributes& dst); + struct NodeCache { struct Entry { NodeId id; uint64_t timestamp; - Eigen::Vector3f init_pos; + //! Odometric position (the unmerged graph is odometric by invariant); doubles + //! as the pgmo vertex for control-point matching. + Eigen::Vector3f pos; + //! Most recent odometric->optimized transform applied to this node, if any. + //! Lets merge hooks bring attributes rebuilt from odometric constituents into + //! the optimized frame. + std::optional last_transform; }; Entry* add(NodeId node, const NodeAttributes& attrs); + /** + * @brief Apply the node's most recent deformation transform to the given + * attributes (via NodeAttributes::transform). + * @returns true if a transform was recorded and applied. + */ + bool applyLastTransform(NodeId node, NodeAttributes& attrs) const; + std::map nodes; }; @@ -68,10 +95,14 @@ class DeformationInterpolator { explicit DeformationInterpolator(const Config& config); /** - * @brief Interpolate the node positions based on the deformation graph, using the + * @brief Interpolate the node poses based on the deformation graph, using the * temporally closest control points as reference. * - * @param unmerged Unmerged scene graph to keep up to date. + * Reads odometric values from the unmerged scene graph and writes the deformed + * (optimized) values into the private DSG only; the unmerged graph is never + * modified. + * + * @param unmerged Unmerged (odometric) scene graph to read from. * @param dsg Private DSG to update. * @param info Update information containing deformation graph. * @param view View on the unmerged scene graph selecting all nodes to update. @@ -81,15 +112,7 @@ class DeformationInterpolator { const UpdateInfo::ConstPtr& info, const LayerView& view) const; - /** - * @brief Interpolate the node positions based on the deformation graph, using the - * temporally closest control points as reference. - * - * @param unmerged Unmerged scene graph to keep up to date. - * @param dsg Private DSG to update. - * @param info Update information containing deformation graph. - * @param view View on the unmerged scene graph selecting all nodes to update. - */ + //! @copydoc DeformationInterpolator::interpolate void interpolateNodePositions(const DynamicSceneGraph& unmerged, DynamicSceneGraph& dsg, const UpdateInfo::ConstPtr& info, @@ -97,6 +120,16 @@ class DeformationInterpolator { interpolate(unmerged, dsg, info, view); } + /** + * @brief Apply the node's most recent deformation transform to the given + * attributes (e.g. to bring merge-hook output rebuilt from odometric + * constituents into the optimized frame). + * @returns true if a transform was recorded and applied. + */ + bool applyLastTransform(NodeId node, NodeAttributes& attrs) const { + return cache_.applyLastTransform(node, attrs); + } + private: mutable NodeCache cache_; }; diff --git a/include/hydra/backend/dsg_updater.h b/include/hydra/backend/dsg_updater.h index ada7e8fa..f7bd1856 100644 --- a/include/hydra/backend/dsg_updater.h +++ b/include/hydra/backend/dsg_updater.h @@ -95,6 +95,8 @@ class DsgUpdater { GroupedMergeTracker merge_tracker; std::map update_functors_; + //! Source (unmerged) graph holding odometric poses; update functors read it and + //! write optimized results into target_dsg_ only (see BackendModule::unmerged_graph_) DynamicSceneGraph::Ptr source_graph_; SharedDsgInfo::Ptr target_dsg_; }; diff --git a/src/backend/deformation_interpolator.cpp b/src/backend/deformation_interpolator.cpp index 0f3d1147..e9cbd095 100644 --- a/src/backend/deformation_interpolator.cpp +++ b/src/backend/deformation_interpolator.cpp @@ -60,6 +60,31 @@ std::string printTransform(const Eigen::Isometry3d& tf) { } // namespace using spark_dsg::NodeSymbol; +using spark_dsg::ObjectNodeAttributes; +using spark_dsg::SemanticNodeAttributes; + +void applyNodeDeformation(const Eigen::Isometry3d& transform, + const NodeAttributes& src, + NodeAttributes& dst) { + dst.position = transform * src.position; + + // transform a fresh copy of the odometric box (never the live optimized box) so + // repeated deformation of a node -- active or archived -- never compounds + const auto src_semantic = dynamic_cast(&src); + auto dst_semantic = dynamic_cast(&dst); + if (src_semantic && dst_semantic && + src_semantic->bounding_box.type != BoundingBox::Type::INVALID) { + dst_semantic->bounding_box = src_semantic->bounding_box; + dst_semantic->bounding_box.transform(transform); + } + + const auto src_object = dynamic_cast(&src); + auto dst_object = dynamic_cast(&dst); + if (src_object && dst_object) { + dst_object->world_R_object = Eigen::Quaterniond( + transform.linear() * src_object->world_R_object.toRotationMatrix()); + } +} void declare_config(DeformationInterpolator::Config& config) { using namespace config; @@ -85,20 +110,31 @@ NodeCache::Entry* NodeCache::add(NodeId node_id, const NodeAttributes& attrs) { auto iter = nodes.find(node_id); if (iter == nodes.end()) { - return &nodes - .emplace(node_id, - Entry{node_id, - attrs.last_update_time_ns, - attrs.position.cast()}) - .first->second; + iter = + nodes + .emplace( + node_id, + Entry{ + node_id, timestamp_ns, attrs.position.cast(), std::nullopt}) + .first; + return &iter->second; } - if (attrs.is_active) { - iter->second.init_pos = attrs.position.cast(); - iter->second.timestamp = attrs.last_update_time_ns; + // the attributes come from the unmerged graph, which is odometric by invariant, + // so the cached values can always refresh (archived nodes never change anyway) + iter->second.pos = attrs.position.cast(); + iter->second.timestamp = timestamp_ns; + return &iter->second; +} + +bool NodeCache::applyLastTransform(NodeId node_id, NodeAttributes& attrs) const { + const auto iter = nodes.find(node_id); + if (iter == nodes.end() || !iter->second.last_transform) { + return false; } - return &iter->second; + attrs.transform(*iter->second.last_transform); + return true; } struct EntryList { @@ -126,7 +162,7 @@ kimera_pgmo::traits::Pos pgmoGetVertex(const EntryList& entries, traits->stamp = entry->timestamp; } - return entry->init_pos; + return entry->pos; } uint64_t pgmoGetVertexStamp(const EntryList& entries, size_t i) { @@ -189,14 +225,19 @@ void DeformationInterpolator::interpolate(const DynamicSceneGraph& unmerged, VLOG(5) << "node " << spark_dsg::NodeSymbol(entry->id).str() << " -> transform: " << printTransform(transform); - const auto new_pos = transform * entry->init_pos.cast(); - auto& attrs = unmerged.getNode(entry->id).attributes(); - attrs.position = new_pos; + // recorded so merge hooks can bring attributes rebuilt from odometric + // constituents into the optimized frame + entry->last_transform = transform; auto node_ptr = dsg.findNode(entry->id); - if (node_ptr) { - node_ptr->attributes().position = new_pos; + if (!node_ptr) { + return; } + + // the unmerged graph stays odometric: read the odometric source values and + // write the deformed results into the merged graph only + const auto& src = unmerged.getNode(entry->id).attributes(); + applyNodeDeformation(transform, src, node_ptr->attributes()); }; dgraph.customDeformation(deform_func, diff --git a/src/backend/update_region_growing_traversability_functor.cpp b/src/backend/update_region_growing_traversability_functor.cpp index f30e7a47..2a975b5f 100644 --- a/src/backend/update_region_growing_traversability_functor.cpp +++ b/src/backend/update_region_growing_traversability_functor.cpp @@ -77,7 +77,14 @@ UpdateFunctor::Hooks UpdateRegionGrowingTraversabilityFunctor::hooks() const { }; my_hooks.merge = [this](const DynamicSceneGraph& dsg, const std::vector& merge_ids) { - return mergeNodes(dsg, merge_ids); + auto attrs = mergeNodes(dsg, merge_ids); + if (attrs) { + // the merged attributes are cloned from the odometric unmerged graph; + // re-apply the surviving node's last deformation so the merge result stays + // in the optimized frame + deformation_interpolator_.applyLastTransform(merge_ids.front(), *attrs); + } + return attrs; }; my_hooks.cleanup = [this](const UpdateInfo::ConstPtr& info, DynamicSceneGraph&, diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 7c8b00a9..cea423c8 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -10,6 +10,7 @@ add_executable( test_${PROJECT_NAME} main.cpp src/resources.cpp + backend/test_deformation_interpolator.cpp backend/test_dsg_updater.cpp backend/test_external_loop_closure.cpp backend/test_generic_update_functor.cpp diff --git a/tests/backend/test_deformation_interpolator.cpp b/tests/backend/test_deformation_interpolator.cpp new file mode 100644 index 00000000..447d6f71 --- /dev/null +++ b/tests/backend/test_deformation_interpolator.cpp @@ -0,0 +1,194 @@ +#include +#include + +#include "hydra/backend/deformation_interpolator.h" + +namespace hydra { + +using spark_dsg::BoundingBox; +using spark_dsg::KhronosObjectAttributes; +using spark_dsg::NodeAttributes; +using spark_dsg::ObjectNodeAttributes; +using spark_dsg::SemanticNodeAttributes; + +namespace { + +Eigen::Isometry3d makeTransform() { + Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); + transform.linear() = + Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitZ()).toRotationMatrix(); + transform.translation() = Eigen::Vector3d(10.0, -2.0, 1.0); + return transform; +} + +} // namespace + +// Khronos objects never populate last_update_time_ns (it stays 0), so NodeCache +// must fall back to last_observed_ns. Otherwise the cached entry is stamped 0 +// and the deformation solver matches it against the earliest control points +// instead of the ones near the object's observation time. +TEST(NodeCache, KhronosFallbackTimestampOnInsert) { + constexpr uint64_t observed_ns = 1782847154557208075ULL; + KhronosObjectAttributes attrs; + attrs.last_update_time_ns = 0; + attrs.last_observed_ns = {observed_ns}; + attrs.is_active = true; + + NodeCache cache; + const NodeId node_id = 42; + + auto* entry = cache.add(node_id, attrs); + ASSERT_NE(entry, nullptr); + EXPECT_EQ(entry->timestamp, observed_ns); + + // re-adding an active node must keep the fallback stamp, not reset it to 0 + entry = cache.add(node_id, attrs); + ASSERT_NE(entry, nullptr); + EXPECT_EQ(entry->timestamp, observed_ns); +} + +// The unmerged graph holds odometric values by invariant, so the cached position +// (used as the pgmo vertex for control-point matching) refreshes on every add, +// including for archived nodes. +TEST(NodeCache, RefreshesPositionEvenWhenArchived) { + NodeAttributes attrs; + attrs.last_update_time_ns = 10u; + attrs.is_active = true; + attrs.position = Eigen::Vector3d(1.0, 1.0, 1.0); + + NodeCache cache; + const NodeId node_id = 8; + cache.add(node_id, attrs); + + attrs.is_active = false; + attrs.position = Eigen::Vector3d(2.0, 3.0, 4.0); + auto* entry = cache.add(node_id, attrs); + ASSERT_NE(entry, nullptr); + EXPECT_TRUE(entry->pos.isApprox(attrs.position.cast())); +} + +// Merge hooks rebuild parent attributes from odometric constituents, so without a +// recorded deformation there is nothing to apply and the attributes stay untouched. +TEST(NodeCache, ApplyLastTransformNoEntryIsNoop) { + NodeCache cache; + + NodeAttributes attrs; + attrs.position = Eigen::Vector3d(1.0, 2.0, 3.0); + EXPECT_FALSE(cache.applyLastTransform(3, attrs)); + EXPECT_TRUE(attrs.position.isApprox(Eigen::Vector3d(1.0, 2.0, 3.0))); + + // an entry that was never deformed also applies nothing + attrs.last_update_time_ns = 10u; + cache.add(3, attrs); + EXPECT_FALSE(cache.applyLastTransform(3, attrs)); + EXPECT_TRUE(attrs.position.isApprox(Eigen::Vector3d(1.0, 2.0, 3.0))); +} + +// Once the deform callback records a node's odometric->optimized transform, merge +// hooks can bring freshly-cloned odometric attributes into the optimized frame. +TEST(NodeCache, ApplyLastTransformAppliesStoredTransform) { + const auto transform = makeTransform(); + + ObjectNodeAttributes attrs; + attrs.last_update_time_ns = 10u; + attrs.is_active = true; + attrs.position = Eigen::Vector3d(1.0, 2.0, 3.0); + attrs.bounding_box = + BoundingBox(Eigen::Vector3f(1.0f, 2.0f, 3.0f), Eigen::Vector3f(1.0f, 2.0f, 3.0f)); + attrs.world_R_object = Eigen::Quaterniond::Identity(); + + NodeCache cache; + const NodeId node_id = 4; + cache.add(node_id, attrs); + cache.nodes.at(node_id).last_transform = transform; + + auto expected_box = attrs.bounding_box; + expected_box.transform(transform); + + auto merged = attrs.clone(); + EXPECT_TRUE(cache.applyLastTransform(node_id, *merged)); + + const auto& result = dynamic_cast(*merged); + EXPECT_TRUE(result.position.isApprox(transform * attrs.position)); + EXPECT_TRUE(result.bounding_box.world_P_center.isApprox(expected_box.world_P_center)); + EXPECT_TRUE( + result.world_R_object.toRotationMatrix().isApprox(transform.linear(), 1.0e-9)); +} + +// The deform callback derives every optimized field from the odometric source, so +// applying the same transform repeatedly to an already-deformed destination must +// not compound (archived merged nodes are re-deformed every post-LC spin). +TEST(ApplyNodeDeformation, DerivesFromSourceIdempotently) { + const auto transform = makeTransform(); + + ObjectNodeAttributes src; + src.position = Eigen::Vector3d(1.0, 2.0, 3.0); + src.bounding_box = + BoundingBox(Eigen::Vector3f(2.0f, 4.0f, 6.0f), Eigen::Vector3f(1.0f, 2.0f, 3.0f)); + src.world_R_object = Eigen::Quaterniond::Identity(); + + auto dst_ptr = src.clone(); + auto& dst = dynamic_cast(*dst_ptr); + // simulate stale optimized state on the destination + dst.position = Eigen::Vector3d(9.0, 9.0, 9.0); + dst.bounding_box = + BoundingBox(Eigen::Vector3f(9.0f, 9.0f, 9.0f), Eigen::Vector3f(9.0f, 9.0f, 9.0f)); + + auto expected_box = src.bounding_box; + expected_box.transform(transform); + + applyNodeDeformation(transform, src, dst); + applyNodeDeformation(transform, src, dst); // second application must not compound + + EXPECT_TRUE(dst.position.isApprox(transform * src.position)); + EXPECT_TRUE(dst.bounding_box.world_P_center.isApprox(expected_box.world_P_center)); + EXPECT_TRUE(dst.bounding_box.dimensions.isApprox(expected_box.dimensions)); + EXPECT_TRUE( + dst.world_R_object.toRotationMatrix().isApprox(transform.linear(), 1.0e-9)); +} + +// Merged-only state (e.g. merge-accumulated mesh connections) must survive +// deformation: only spatial fields are written. +TEST(ApplyNodeDeformation, PreservesNonSpatialDestinationState) { + const auto transform = makeTransform(); + + KhronosObjectAttributes src; + src.position = Eigen::Vector3d(1.0, 2.0, 3.0); + src.mesh_connections = {1, 2}; + + auto dst_ptr = src.clone(); + auto& dst = dynamic_cast(*dst_ptr); + dst.mesh_connections = {1, 2, 3, 4}; // accumulated by merges + + applyNodeDeformation(transform, src, dst); + + EXPECT_TRUE(dst.position.isApprox(transform * src.position)); + EXPECT_EQ(dst.mesh_connections, decltype(dst.mesh_connections)({1, 2, 3, 4})); +} + +// Nodes without a bounding box (plain NodeAttributes, e.g. agent poses) only get +// their position updated; an INVALID source box is never copied over. +TEST(ApplyNodeDeformation, SkipsBoxForPlainAttributes) { + const auto transform = makeTransform(); + + NodeAttributes src; + src.position = Eigen::Vector3d(1.0, 2.0, 3.0); + + NodeAttributes dst; + applyNodeDeformation(transform, src, dst); + EXPECT_TRUE(dst.position.isApprox(transform * src.position)); + + // semantic destination keeps its box when the source box is INVALID + SemanticNodeAttributes semantic_src; + semantic_src.position = Eigen::Vector3d(1.0, 2.0, 3.0); + + SemanticNodeAttributes semantic_dst; + semantic_dst.bounding_box = + BoundingBox(Eigen::Vector3f(1.0f, 1.0f, 1.0f), Eigen::Vector3f(0.0f, 0.0f, 0.0f)); + applyNodeDeformation(transform, semantic_src, semantic_dst); + EXPECT_EQ(semantic_dst.bounding_box.type, BoundingBox::Type::AABB); + EXPECT_TRUE( + semantic_dst.bounding_box.dimensions.isApprox(Eigen::Vector3f(1.0f, 1.0f, 1.0f))); +} + +} // namespace hydra diff --git a/tests/backend/test_generic_update_functor.cpp b/tests/backend/test_generic_update_functor.cpp index 936dc65c..42fe8859 100644 --- a/tests/backend/test_generic_update_functor.cpp +++ b/tests/backend/test_generic_update_functor.cpp @@ -109,15 +109,25 @@ TEST(GenericUpdateFunctor, shouldUpdate) { VLOG(1) << "Using config:\n" << config::toString(config); GenericUpdateFunctor functor(config); - callWithUnmerged(functor, *dsg, info, false); + // the unmerged graph persists across spins and stays odometric; the functor only + // ever writes the merged graph + const auto unmerged = graph.clone(); + functor.call(*unmerged, *dsg, info); const auto& result = graph.getNode(0).attributes(); const Eigen::Vector3d expected(1.0, 2.0, 3.0); EXPECT_NEAR(0.0, (expected - result.position).norm(), 1.0e-7); - // second call shouldn't change position + { // the odometric source is never touched + const Eigen::Vector3d odometric(0.0, 3.0, 1.0); + const auto& src = unmerged->getNode(0).attributes(); + EXPECT_NEAR(0.0, (odometric - src.position).norm(), 1.0e-7); + } + + // re-deforming an archived node must not compound graph.getNode(0).attributes().is_active = false; - callWithUnmerged(functor, *dsg, info, false); + unmerged->getNode(0).attributes().is_active = false; + functor.call(*unmerged, *dsg, info); EXPECT_NEAR(0.0, (expected - result.position).norm(), 1.0e-7); } From cbc552e5be86555624a43a76c3a46a6023771805 Mon Sep 17 00:00:00 2001 From: harelb Date: Tue, 7 Jul 2026 23:32:59 -0400 Subject: [PATCH 6/7] fix(backend): find pass-0 merges on the optimized merged graph Pass-0 merge finding ran on the source (unmerged) graph, which is odometric: duplicates revealed by a loop closure only overlap in the optimized frame, so post-LC duplicates were never proposed. Run pass 0 on the target graph like the exhaustive passes already do (applyMerges already remaps proposals through prior merges). Move the forced-active hack before the graph merge so NEW-but-archived nodes are also active in the target graph's views, and restore the flag on the target copies at the end of the spin (they are never re-cloned once the source node is archived again, so the flag would otherwise stick forever). --- src/backend/dsg_updater.cpp | 27 +++++++++++----- tests/backend/test_dsg_updater.cpp | 50 ++++++++++++++++++++++++++++++ 2 files changed, 69 insertions(+), 8 deletions(-) diff --git a/src/backend/dsg_updater.cpp b/src/backend/dsg_updater.cpp index 9899c3a5..3e9123c2 100644 --- a/src/backend/dsg_updater.cpp +++ b/src/backend/dsg_updater.cpp @@ -56,7 +56,9 @@ void findAndApplyMerges(const VerbosityConfig& config, MergeTracker& tracker, bool exhaustive) { // TODO(nathan) handle given merges - auto merges = hooks.find_merges(source, info); + // merges are found on the target (merged) graph: the source graph is odometric, + // and duplicates revealed by a loop closure only overlap in the optimized frame + auto merges = hooks.find_merges(*target.graph, info); auto applied = tracker.applyMerges(source, merges, target, hooks.merge); MLOG(1) << "pass 0: " << merges.size() << " merges (applied " << applied << ")"; if (!exhaustive) { @@ -164,14 +166,10 @@ void DsgUpdater::callUpdateFunctions(size_t timestamp_ns, UpdateInfo::ConstPtr i resetBackendDsg(timestamp_ns); } - GraphMergeConfig merge_config; - merge_config.previous_merges = &target_dsg_->merges; - merge_config.update_dynamic_attributes = false; - target_dsg_->graph->mergeGraph(*source_graph_, merge_config); - // Nodes occasionally get added to the backend after they've left the active window, // which means they never get deformed or updated correctly. This forces them to be - // active for at least one update + // active for at least one update. Runs before the graph merge so the nodes are + // also active in the target (merged) graph's views (e.g. for merge finding). std::vector active_nodes_to_restore; for (auto& [layer_id, layer] : source_graph_->layers()) { for (auto& [node_id, node] : layer->nodes()) { @@ -183,6 +181,11 @@ void DsgUpdater::callUpdateFunctions(size_t timestamp_ns, UpdateInfo::ConstPtr i } } + GraphMergeConfig merge_config; + merge_config.previous_merges = &target_dsg_->merges; + merge_config.update_dynamic_attributes = false; + target_dsg_->graph->mergeGraph(*source_graph_, merge_config); + const std::set exhaustive_names(config.exhaustive_functors.begin(), config.exhaustive_functors.end()); std::list cleanup_hooks; @@ -221,12 +224,20 @@ void DsgUpdater::callUpdateFunctions(size_t timestamp_ns, UpdateInfo::ConstPtr i func(info, *source_graph_, target_dsg_.get()); } - // We reset active flags for all new nodes that were inactive after one update + // We reset active flags for all new nodes that were inactive after one update. + // The graph merge cloned the forced-active flag into the target graph, and the + // target copy is never re-cloned once the source node is archived again, so the + // flag has to be restored on both graphs. for (auto node_id : active_nodes_to_restore) { auto node = source_graph_->findNode(node_id); if (node) { node->attributes().is_active = false; } + + auto target_node = target_dsg_->graph->findNode(node_id); + if (target_node) { + target_node->attributes().is_active = false; + } } // clear new node status diff --git a/tests/backend/test_dsg_updater.cpp b/tests/backend/test_dsg_updater.cpp index d609821b..8755389d 100644 --- a/tests/backend/test_dsg_updater.cpp +++ b/tests/backend/test_dsg_updater.cpp @@ -36,6 +36,7 @@ #include #include #include +#include #include "hydra_test/shared_dsg_fixture.h" @@ -140,4 +141,53 @@ TEST(DsgUpdater, PerSpinBookkeepingSharedAcrossFunctors) { EXPECT_FALSE(source->getNode(trackedNode()).attributes().is_active); } +// Duplicates revealed by a loop closure only overlap in the OPTIMIZED frame; the +// unmerged (source) graph stays odometric, so pass-0 merge finding has to run on +// the merged graph like the exhaustive passes already do. +TEST(DsgUpdater, FindsPassZeroMergesOnMergedGraph) { + auto target = test::makeSharedDsg(); + DynamicSceneGraph::Ptr source = target->graph->clone(); + + // two archived objects far apart in the odometric (source) frame + const auto node_a = spark_dsg::NodeSymbol('o', 0); + const auto node_b = spark_dsg::NodeSymbol('o', 1); + for (const auto node_id : {node_a, node_b}) { + auto attrs = std::make_unique(); + attrs->position = + Eigen::Vector3d(10.0 * spark_dsg::NodeSymbol(node_id).categoryId(), 0.0, 0.0); + attrs->is_active = false; + attrs->last_update_time_ns = 10u; + source->emplaceNode(DsgLayers::OBJECTS, node_id, std::move(attrs)); + } + + GenericUpdateFunctor::Config functor_config; + functor_config.layer = DsgLayers::OBJECTS; + functor_config.enable_merging = true; + + DsgUpdater::Config config; + config.update_functors.emplace_back( + "objects", DsgUpdater::Config::FunctorConfig(functor_config)); + + DsgUpdater updater(config, source, target); + + // first spin clones both nodes into the merged graph + UpdateInfo::ConstPtr info(new UpdateInfo{0}); + updater.callUpdateFunctions(0, info); + ASSERT_TRUE(target->graph->hasNode(node_a)); + ASSERT_TRUE(target->graph->hasNode(node_b)); + + // simulate optimization: the merged copies land on top of each other (archived + // nodes are not reset by the graph merge, so this persists across spins) + target->graph->getNode(node_a).attributes().position = Eigen::Vector3d::Zero(); + target->graph->getNode(node_b).attributes().position = + Eigen::Vector3d(0.05, 0.0, 0.0); + + // loop-closure spin: the duplicates only overlap in the merged graph + UpdateInfo::ConstPtr lc_info(new UpdateInfo{1, nullptr, nullptr, true, {}}); + updater.callUpdateFunctions(1, lc_info); + + EXPECT_EQ(target->merges.size(), 1u); + EXPECT_EQ(target->graph->getLayer(DsgLayers::OBJECTS).numNodes(), 1u); +} + } // namespace hydra From 703d7d65a9bae3b286b2475805b16400d16810dd Mon Sep 17 00:00:00 2001 From: harelb Date: Tue, 7 Jul 2026 23:08:44 -0400 Subject: [PATCH 7/7] fix(tests): find object merges on the merged graph The image-storage rework of UpdateObjectsFunctor::call stopped writing mesh-derived geometry back into the unmerged graph (clone-first), so find_merges run against the unmerged graph sees stale zero-extent boxes and proposes nothing. Merges are found on the merged graph, which is where the updated geometry lives; point the test helper there to match (the DsgUpdater pass-0 call site changes separately). --- tests/backend/test_update_objects_functor.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/tests/backend/test_update_objects_functor.cpp b/tests/backend/test_update_objects_functor.cpp index 069b76b5..09d99a5f 100644 --- a/tests/backend/test_update_objects_functor.cpp +++ b/tests/backend/test_update_objects_functor.cpp @@ -49,7 +49,9 @@ MergeList callWithUnmerged(const UpdateFunctor& functor, functor.call(*unmerged, dsg, info); const auto hooks = functor.hooks(); if (enable_merging && hooks.find_merges) { - return hooks.find_merges(*unmerged, info); + // mirror DsgUpdater: merges are found on the merged graph, which carries the + // updated (optimized) geometry; the unmerged graph stays odometric + return hooks.find_merges(*dsg.graph, info); } else { return {}; }